Small demonstration of the move-to-point motion algorithm.

mtp.cpp

/**
 * Given a left, right, and maximum speed, calculate a new set of speed values that preserve the
 * ratio between left and right if one of the two values is over the maximum threshold.
 * 
 * @param left_speed The uncapped left speed.
 * @param right_speed The uncapped right speed.
 * @param max_speed The maximum speed that can be reached by a motor.
 * 
 * @return A pair of speed values {left_speed, right_speed} that is correctly scaled down if one
 * of the two values exceeds max_speed.
*/
std::pair<double, double> normalize_speeds(double left_speed, double right_speed, double max_speed) {
	double largest_speed = std::max(std::abs(left_speed), std::abs(right_speed)) / max_speed;

	if (largest_speed > 1.0) {
		left_speed /= largest_speed;
		right_speed /= largest_speed;
	}

	return { left_speed, right_speed };
}

/**
 * Converts an angle in radians to an angle in degrees.
 *
 * @param radians The angle in radians.
 * @return The angle in degrees.
 */
constexpr double to_degrees(double radians) {
	return radians * (180.0 / PI);
}

/**
 * Converts an angle in degrees to an angle in radians.
 *
 * @param degrees The angle in degrees.
 * @return The angle in radians.
 */
constexpr double to_radians(double degrees) {
	return degrees * (PI / 180.0);
}

void moveToPoint(Point target_position) {
	// We'll use a 2 inch tolerance for determing when we settled. This basically means the movement will get 
	// cut off after we're 2 inches from the target.
	constexpr double TOLERANCE = 2.0;

	while (true) {
		// This is just the distance formula between our current odom position and the target point
		double drive_error = std::sqrt(
			std::pow(target_position.x - position.x, 2.0),
			std::pow(target_position.y - position.y, 2.0)
		);

		// If our distance becomes less than our tolerance, we're done!
		if (drive_error < TOLERANCE) {
			break;	
		}

		// Calculate the angle between our current position and our target position.
		//
		// This is basically the angle that we have to turn by in order to be facing our target point. A few notes:
		// - std::remainder normalizes the angle from -180 to 180, so we always turn using the most efficient direction.
		// - std::atan2 gives the theta value of a given point. essentially, it solves for an angle imagine the x and y
		// 	 were the base/height of a right triangle.
		double turn_error = std::remainder(heading - to_degrees(std::atan2(target_position.y - position.y, target_position.x - position.x)), 360.0);

		// If the turn error exceeds 90 degrees, then the point is behind the
		// robot, so it's more efficient to travel to the point backwards.
		if (std::abs(turn_error) >= 90.0) {
			turn_error = std::remainder(turn_error - 180.0, 360.0);
			drive_error *= -1.0;
		}

		// Update PID controllers with our new errors.
		double drive_voltage = drive_pid.calculate(drive_error);
		double turn_voltage = turn_pid.calculate(turn_error);

		// Scale drive output by the cosine of turn error to throttle driving at the start of the movement so we prioritize turning instead.
		drive_voltage *= cos(to_radians(turn_error));

		// Convert our drive and turn voltage into left/right voltages, then normalize these voltages to protect against
		// oversaturation at more than 12 volts.
		std::pair<double, double> voltages = normalize_speeds(
			drive_voltage + turn_voltage,
			drive_voltage - turn_voltage,
			12.0
		);

		// Spin motors!
		left_motors.spin(vex::forward, voltages.0, vex::volt);
		left_motors.spin(vex::forward, voltages.1, vex::volt);

		vex::wait(10, vex::seconds);
	}
}