While this code is nothing special in terms of robotics innovation, being the first major robotics codebase I have led, I'm happy with it.

Here are some of the cool features:

1. Controller rumbles at the sg3 time (last 30sec early season, last 15 sec at world's):

   code/src/main.cpp

   Lines 150 to 185 in f981a61

   	void sg3_warn()
   	{
   	// competition_get_status returns ENABLED,AUTONOMOUS,CONNECTED bits
   	// no its CONNECTED, AUTO, DISABLE so 0b100
   	// the xor (^) function negates bits, i.e. 1 ^ 1 = 0
   	// Here, 0b101, ENABLED,NO_AUTON,CONNECTED is my wanted state
   	// when it reaches the wanted state (i.e.) status returned is 0b101, the answer is 0
   	// until the answer is 0 (i.e. while ans != 0), it loops and waits
   	while ((pros::c::competition_get_status() ^ (0b1100)) != 0)
   	{
   	pros::delay(1000);
   	}
   	// printf("SG# FINISH DELAYING\n");
   	// after it finishes looping, i.e. when driver control starts
   	// we need to wait for 1:45 - 0:25 = 1:25 and then buzz at 40s
   	pros::delay((1_min + 20_s).convert(1_ms));
   	// first rumble partner to pre-warn, then rumble main controller, then re-rumble partner to remind
   	int retries = 0;
   	do
   	{
   	errno = 0;
   	pros::c::controller_rumble(pros::E_CONTROLLER_MASTER, ". -- .");
   	// printf("SG# err %d\n", errno);
   	pros::delay(100);
   	retries++;
   	} while (errno != 0 && retries < 9);
   	do
   	{
   	errno = 0;
   	pros::c::controller_rumble(pros::E_CONTROLLER_PARTNER, "- --");
   	pros::delay(100);
   	retries++;
   	} while (errno != 0 && retries < 9);
   	}

2. Auto MOGO grab using Vision and Ultrasonic:

   code/src/autons/vision.hpp

   Lines 5 to 50 in f981a61

   	void front_line_up(okapi::QTime timeout = 4_s, Vision::MOGO type = Vision::ANY, bool otherline = false)
   	{
   	auto distancecontroller = okapi::IterativePosPIDController({0.0003, 0.00019, 0.000005}, okapi::TimeUtilFactory().withSettledUtilParams(15, 9999, 150_ms));
   	auto anglecontroller = okapi::IterativePosPIDController({0.00020 * 2, 0, 0.000001}, okapi::TimeUtilFactory().withSettledUtilParams(15, 5, 150_ms));
   	auto [_, _1, _2, _3, w] = Visions[Vision::FRONT]->get_mogo(type);
   	anglecontroller.setTarget(158);
   	distancecontroller.setTarget(30);
   	int count = 0;
   	do
   	{
   	count++;
   	errno = 0;
   	auto [_, top, x_mid, y_mid, _1] = Visions[Vision::FRONT]->get_mogo(w);
   	auto geterr = errno;
   	auto distvel = -distancecontroller.step(ultrasonic.get());
   	auto yawvel = -1 * anglecontroller.step(x_mid);
   	// yawvel = 0;
   	if (geterr != 0)
   	{
   	yawvel = 0;
   	// distvel = 0;
   	}
   	if (otherline)
   	{
   	if (drive.chassis->getState().y > 4_tile - 6_in)
   	{
   	drive.chassis->getModel()->driveVectorVoltage(-12, 0);
   	return;
   	}
   	}
   	drive.chassis->getModel()->driveVectorVoltage(12.0 * distvel, 12.0 * yawvel);
   	pros::delay(20);
   	// printf("LOP: %f %d %f\n", anglecontroller.getProcessValue(), x_mid, yawvel);
   	// printf("LOP: %f %d %f\n", distancecontroller.getProcessValue(), ultrasonic.get(), distvel);
   	// printf("%d %d\n", anglecontroller.isSettled(), distancecontroller.isSettled());
   	} while (!((anglecontroller.isSettled() && distancecontroller.isSettled()) || count > timeout.convert(1_ms) / 20.0));
   	anglecontroller.flipDisable();
   	distancecontroller.flipDisable();
   	}

   (I would NOT reuse this code as is, it's a mess and could be architected more modularly)

3. Monitor when the PID motion is stuck on a wall or MOGO, and move on:

   code/src/autons/skills.hpp

   Lines 88 to 107 in f981a61

   	void monitorStuckage()
   	{
   	printf("STUCKAGE\n");
   	int count = 0;
   	while (!drive.chassis->isSettled())
   	{
   	if (drive.getForce() > 35_n)
   	count++;
   	else if (count > 0)
   	count -= 1;
   	// 1 sec of stuck
   	if (count > 10)
   	drive.chassis->stop();
   	// stop will cause return on next loop
   	printf("STUCKCOUNT %d\n", count);
   	pros::delay(100);
   	}
   	}

   and

   code/src/subsystems/drivetrain.hpp

   Lines 55 to 63 in f981a61

   	okapi::QForce getForce()
   	{
   	double sum = 0;
   	for (auto i : HARDWARE::drive_motors_left)
   	sum += i.getTorque();
   	for (auto i : HARDWARE::drive_motors_right)
   	sum += i.getTorque();
   	return sum * HARDWARE::drive_gearset.ratio * okapi::newton * okapi::meter / HARDWARE::drive_chassis_scale.middleWheelDiameter;
   	}

   (Again, don't use this, it could have a much better API)

4. My basic neutral goal grab autonomous that FINALLY worked reliably at World's:

   code/src/autonomous.hpp

   Lines 6 to 28 in f981a61

   	void basic_goal_grabe(bool mode) // mode isn't read
   	{
   	drive.chassis->setState(okapi::OdomState{x : 0_tile, y : 1_tile - 1_in});
   	claw.Leave();
   	drive.setMaxVelocity(600);
   	// drive.chassis->model().tank(.8, .8);
   	drive.chassis->model().left(1);
   	drive.chassis->model().right(1);
   	while (drive.chassis->getState().y < 3_tile - 6_in - 14_in)
   	pros::delay(10);
   	drive.chassis->model().left(.3);
   	drive.chassis->model().right(.3);
   	while (drive.chassis->getState().y < 3_tile - 6_in)
   	pros::delay(10);
   	drive.chassis->model().tank(0, 0);
   	claw.Clasp();
   	skillsn::moveDistance(-2_tile);
   	}

   (moral of the story don't use sensors when you only need to go straight, build a better drivetrain)

HUGE THANKS to:

1. OkapiLib folks (for literally all the complex drive, PID, odometry this does)
2. PROS makers (for the framework)
3. EZ-Template (for the auton-selector and ideas about stuck monitoring)
4. PROS-Grafana-CLI (while I didn't use this too much here, I'm really excited about its potential for spin up)
5. Belton High School for funding this, and Mr Giustino for teaching us and enabling us to do this
6. Finally, Vex and RECF for making this even exist

---

To see code history: https://github.com/vex-76513/code/commits/pros

This requires a custom version of OkapiLib at ../OkapiLib from https://github.com/vex-76513/vex-76513-OkapiLib/commit/3c5879de0cd2fa5e4e163e399a040e8aa5de1798 And EZ-Template at ../EZ-Template from https://github.com/EZ-Robotics/EZ-Template/commit/aeaf1785d5a580da01bc50094d1646d7a82383dc

To init environment:

        pros c apply --install --download --force --force-apply kernel@3.5.4