Move WHEEL_ROTS to SwerveController, use structs for SwerveController…

… methods

src/Constants.cpp

@@ -1,7 +1,6 @@
 #include "Constants.h"
 
-#include "Globals.h"
-#include "world_interface/world_interface.h"
+#include "kinematics/DiffDriveKinematics.h"
 
 using namespace control;
 using control::DriveMode;
@@ -50,26 +49,6 @@ const double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_SEC;
 const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE)
 							  .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2);
 
-namespace Drive {
-
-namespace {
-constexpr std::array<int32_t, 4> NORMAL_WHEEL_ROTS = {0, 0, 0, 0};
-const swervewheelvel_t TURN_IN_PLACE_VEL =
-	Globals::swerveController.swerveKinematics().robotVelToWheelVel(0, 0, 1);
-const std::array<int32_t, 4> TURN_IN_PLACE_WHEEL_ROTS = {
-	static_cast<int32_t>(TURN_IN_PLACE_VEL.lfRot * 1000),
-	static_cast<int32_t>(TURN_IN_PLACE_VEL.rfRot * 1000),
-	static_cast<int32_t>(TURN_IN_PLACE_VEL.lbRot * 1000),
-	static_cast<int32_t>(TURN_IN_PLACE_VEL.rbRot * 1000)};
-constexpr std::array<int32_t, 4> CRAB_WHEEL_ROTS = {90000, 90000, 90000, 90000};
-} // namespace
-
-const extern std::unordered_map<DriveMode, std::array<int32_t, 4>> WHEEL_ROTS = {
-	{DriveMode::Normal, NORMAL_WHEEL_ROTS},
-	{DriveMode::TurnInPlace, TURN_IN_PLACE_WHEEL_ROTS},
-	{DriveMode::Crab, CRAB_WHEEL_ROTS}};
-} // namespace Drive
-
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
 const char* MAST_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";

src/Constants.h

@@ -95,9 +95,6 @@ constexpr std::array<robot::types::motorid_t, 4> WHEEL_IDS = {
 	robot::types::motorid_t::rearLeftWheel,
 	robot::types::motorid_t::rearRightWheel,
 };
-
-const extern std::unordered_map<DriveMode, std::array<int32_t, 4>> WHEEL_ROTS;
-constexpr double STEER_EPSILON = 1000;
 } // namespace Drive
 
 namespace Nav {

src/control/SwerveController.cpp

@@ -7,8 +7,27 @@ using namespace control;
 using Globals::swerveController;
 using robot::types::motorid_t;
 
-std::pair<double, std::vector<double>>
-SwerveController::setTurnInPlaceCmdVel(double dtheta, std::vector<int> wheel_rots) {
+namespace {
+const std::array<int32_t, 4> NORMAL_WHEEL_ROTS = {0, 0, 0, 0};
+const kinematics::swervewheelvel_t TURN_IN_PLACE_VEL =
+	swerveController.swerveKinematics().robotVelToWheelVel(0, 0, 1);
+const std::array<int32_t, 4> TURN_IN_PLACE_WHEEL_ROTS = {
+	static_cast<int32_t>(TURN_IN_PLACE_VEL.lfRot * 1000),
+	static_cast<int32_t>(TURN_IN_PLACE_VEL.rfRot * 1000),
+	static_cast<int32_t>(TURN_IN_PLACE_VEL.lbRot * 1000),
+	static_cast<int32_t>(TURN_IN_PLACE_VEL.rbRot * 1000)};
+const std::array<int32_t, 4> CRAB_WHEEL_ROTS = {90000, 90000, 90000, 90000};
+} // namespace
+
+control::SwerveController::SwerveController(double baseWidth, double baseLength)
+	: driveMode(DriveMode::Normal, false), swerve_kinematics(baseWidth, baseLength),
+	  crab_kinematics(baseLength),
+	  WHEEL_ROTS({{DriveMode::Normal, NORMAL_WHEEL_ROTS},
+				  {DriveMode::TurnInPlace, TURN_IN_PLACE_WHEEL_ROTS},
+				  {DriveMode::Crab, CRAB_WHEEL_ROTS}}) {}
+
+std::pair<double, swerve_commands_t>
+SwerveController::setTurnInPlaceCmdVel(double dtheta, swerve_rots_t wheel_rots) {
 	if (dtheta != 0) {
 		return {0, {0.0, 0.0, 0.0, 0.0}};
 	}
@@ -35,8 +54,8 @@ SwerveController::setTurnInPlaceCmdVel(double dtheta, std::vector<int> wheel_rot
 	return {maxAbsPWM > 1 ? maxAbsPWM : 1.0, {lfPWM, rfPWM, lbPWM, rbPWM}};
 }
 
-std::pair<double, std::vector<double>>
-SwerveController::setCrabCmdVel(double dtheta, double dy, std::vector<int> wheel_rots) {
+std::pair<double, swerve_commands_t>
+SwerveController::setCrabCmdVel(double dtheta, double dy, swerve_rots_t wheel_rots) {
 	if (Globals::E_STOP && (dtheta != 0 || dy != 0)) {
 		return {0, {0.0, 0.0, 0.0, 0.0}};
 	}
@@ -56,10 +75,10 @@ SwerveController::setCrabCmdVel(double dtheta, double dy, std::vector<int> wheel
 	return {maxAbsPWM > 1 ? maxAbsPWM : 1.0, {rPWM, rPWM, lPWM, lPWM}};
 }
 
-bool SwerveController::checkWheelRotation(DriveMode mode, std::vector<int> wheel_rots) {
+bool SwerveController::checkWheelRotation(DriveMode mode, swerve_rots_t wheel_rots) {
+	int rots[4] = {wheel_rots.lfRot, wheel_rots.rfRot, wheel_rots.lbRot, wheel_rots.rbRot};
 	for (int i = 0; i < 4; i++) {
-		if (std::abs(wheel_rots[i] - Constants::Drive::WHEEL_ROTS.at(mode)[i]) <
-			Constants::Drive::STEER_EPSILON)
+		if (std::abs(rots[i] - WHEEL_ROTS.at(mode)[i]) < STEER_EPSILON)
 			return false;
 	}
 	return true;

src/control/SwerveController.h

@@ -17,53 +17,72 @@ static const std::map<DriveMode, std::string> driveModeStrings = {
 	{DriveMode::TurnInPlace, "TurnInPlace"},
 	{DriveMode::Crab, "Crab"}};
 
+// Represents the rotations of all four steer motors in in millidegrees.
+struct swerve_rots_t {
+	int lfRot;
+	int rfRot;
+	int lbRot;
+	int rbRot;
+};
+
+// The PWM commands to all four steer motors.
+struct swerve_commands_t {
+	double lfPWM;
+	double rfPWM;
+	double lbPWM;
+	double rbPWM;
+};
+
 class SwerveController {
 public:
-	SwerveController(double baseWidth, double baseLength)
-		: driveMode(DriveMode::Normal, false), swerve_kinematics(baseWidth, baseLength),
-		  crab_kinematics(baseLength){};
+	SwerveController(double baseWidth, double baseLength);
 
 	/**
 	 * @brief Request the robot to turn in place using swerve.
 	 *
 	 * @param dtheta The heading velocity.
-	 * @param wheel_rots The rotation of all four wheels in order FL, FR, RL, RR
-	 * @return double If the requested velocities are too high, they will be scaled down.
-	 * The returned value is the scale divisor. If no scaling was performed, 1 is returned.
+	 * @param wheel_rots The rotation of all four wheels in millidegrees
+	 * @return std::pair<double, swerve_commands_t> First: If the requested velocities are too
+	 * high, they will be scaled down. The returned value is the scale divisor. If no scaling
+	 * was performed, 1 is returned. Second: The PWM commands for all four wheels.
 	 */
-	std::pair<double, std::vector<double>> setTurnInPlaceCmdVel(double dtheta,
-																std::vector<int> wheels);
+	std::pair<double, swerve_commands_t> setTurnInPlaceCmdVel(double dtheta,
+															  swerve_rots_t wheels);
 	/**
 	 * @brief Request the robot to drive side to side and turn with given velocities using
 	 * swerve. Essentially rotate the reference frame of the robot by 90 degrees.
 	 *
 	 * @param dtheta The heading velocity.
 	 * @param dy The side-to-side velocity.
-	 * @param wheel_rots The rotation of all four wheels in order FL, FR, RL, RR
-	 * @return double If the requested velocities are too high, they will be scaled down.
-	 * The returned value is the scale divisor. If no scaling was performed, 1 is returned.
+	 * @param wheel_rots The rotation of all four wheels in millidegrees
+	 * @return std::pair<double, swerve_commands_t> First: If the requested velocities are too
+	 * high, they will be scaled down. The returned value is the scale divisor. If no scaling
+	 * was performed, 1 is returned. Second: The PWM commands for all four wheels.
 	 */
-	std::pair<double, std::vector<double>> setCrabCmdVel(double dtheta, double dy,
-														 std::vector<int> wheels);
+	std::pair<double, swerve_commands_t> setCrabCmdVel(double dtheta, double dy,
+													   swerve_rots_t wheels);
 
 	kinematics::SwerveDriveKinematics swerveKinematics();
 
 	/**
 	 * @brief Check to see if all wheels are at their target position.
 	 *
 	 * @param mode The drive mode to compare wheel rotations against.
-	 * @param wheel_rots The rotation of all four wheels in order FL, FR, RL, RR
+	 * @param wheel_rots The rotation of all four wheels in millidegrees
 	 * @return bool if the wheels are within `Constants::Drive::STEER_EPSILON` of their their
 	 * target position.
 	 */
-	bool checkWheelRotation(DriveMode mode, std::vector<int> wheel_rots);
+	bool checkWheelRotation(DriveMode mode, swerve_rots_t wheel_rots);
 
 	// The boolean here is whether or not wheel rotation check should be ignored when doing
 	// swerve-based driving
 	std::pair<DriveMode, bool> driveMode;
 
+	const std::unordered_map<DriveMode, std::array<int32_t, 4>> WHEEL_ROTS;
+
 private:
 	const kinematics::SwerveDriveKinematics swerve_kinematics;
 	const kinematics::DiffDriveKinematics crab_kinematics;
+	const double STEER_EPSILON = 1000;
 };
 } // namespace control

src/network/MissionControlProtocol.cpp

@@ -92,19 +92,20 @@ void MissionControlProtocol::handleDriveModeRequest(const json& j) {
 		swerveController.driveMode.first = DriveMode::Normal;
 		for (int i = 0; i < 4; i++) {
 			robot::setMotorPos(Constants::Drive::WHEEL_IDS[i],
-							   Constants::Drive::WHEEL_ROTS.at(DriveMode::Normal)[i]);
+							   Globals::swerveController.WHEEL_ROTS.at(DriveMode::Normal)[i]);
 		}
 	} else if (mode == "turn-in-place") {
 		swerveController.driveMode.first = DriveMode::TurnInPlace;
 		for (int i = 0; i < 4; i++) {
-			robot::setMotorPos(Constants::Drive::WHEEL_IDS[i],
-							   Constants::Drive::WHEEL_ROTS.at(DriveMode::TurnInPlace)[i]);
+			robot::setMotorPos(
+				Constants::Drive::WHEEL_IDS[i],
+				Globals::swerveController.WHEEL_ROTS.at(DriveMode::TurnInPlace)[i]);
 		}
 	} else if (mode == "crab") {
 		swerveController.driveMode.first = DriveMode::Crab;
 		for (int i = 0; i < 4; i++) {
 			robot::setMotorPos(Constants::Drive::WHEEL_IDS[i],
-							   Constants::Drive::WHEEL_ROTS.at(DriveMode::Crab)[i]);
+							   Globals::swerveController.WHEEL_ROTS.at(DriveMode::Crab)[i]);
 		}
 	}
 
@@ -230,31 +231,31 @@ void MissionControlProtocol::setRequestedTankCmdVel(double left, double right) {
 
 void MissionControlProtocol::setRequestedTurnInPlaceCmdVel(double dtheta) {
 	_power_repeat_task.setTurnInPlaceCmdVel(dtheta);
-	std::vector<int> curr_wheel_rots = {
+	control::swerve_rots_t curr_wheel_rots = {
 		robot::getMotorPos(motorid_t::frontLeftWheel).getData(),
 		robot::getMotorPos(motorid_t::frontRightWheel).getData(),
 		robot::getMotorPos(motorid_t::rearLeftWheel).getData(),
 		robot::getMotorPos(motorid_t::rearRightWheel).getData()};
-	std::vector<double> new_wheel_rots =
+	control::swerve_commands_t steer_PWM =
 		Globals::swerveController.setTurnInPlaceCmdVel(dtheta, curr_wheel_rots).second;
-	robot::setMotorPower(motorid_t::frontLeftWheel, new_wheel_rots[0]);
-	robot::setMotorPower(motorid_t::frontRightWheel, new_wheel_rots[1]);
-	robot::setMotorPower(motorid_t::rearLeftWheel, new_wheel_rots[2]);
-	robot::setMotorPower(motorid_t::rearRightWheel, new_wheel_rots[3]);
+	robot::setMotorPower(motorid_t::frontLeftWheel, steer_PWM.lfPWM);
+	robot::setMotorPower(motorid_t::frontRightWheel, steer_PWM.rfPWM);
+	robot::setMotorPower(motorid_t::rearLeftWheel, steer_PWM.lbPWM);
+	robot::setMotorPower(motorid_t::rearRightWheel, steer_PWM.rbPWM);
 }
 
 void MissionControlProtocol::setRequestedCrabCmdVel(double dtheta, double dy) {
 	_power_repeat_task.setCrabCmdVel(dtheta, dy);
-	std::vector<int> curr_wheel_rots = {robot::getMotorPos(motorid_t::frontLeftWheel),
-										robot::getMotorPos(motorid_t::frontRightWheel),
-										robot::getMotorPos(motorid_t::rearLeftWheel),
-										robot::getMotorPos(motorid_t::rearRightWheel)};
-	std::vector<double> new_wheel_rots =
+	control::swerve_rots_t curr_wheel_rots = {robot::getMotorPos(motorid_t::frontLeftWheel),
+											  robot::getMotorPos(motorid_t::frontRightWheel),
+											  robot::getMotorPos(motorid_t::rearLeftWheel),
+											  robot::getMotorPos(motorid_t::rearRightWheel)};
+	control::swerve_commands_t steer_PWM =
 		Globals::swerveController.setCrabCmdVel(dtheta, dy, curr_wheel_rots).second;
-	robot::setMotorPower(motorid_t::frontLeftWheel, new_wheel_rots[0]);
-	robot::setMotorPower(motorid_t::frontRightWheel, new_wheel_rots[1]);
-	robot::setMotorPower(motorid_t::rearLeftWheel, new_wheel_rots[2]);
-	robot::setMotorPower(motorid_t::rearRightWheel, new_wheel_rots[3]);
+	robot::setMotorPower(motorid_t::frontLeftWheel, steer_PWM.lfPWM);
+	robot::setMotorPower(motorid_t::frontRightWheel, steer_PWM.rfPWM);
+	robot::setMotorPower(motorid_t::rearLeftWheel, steer_PWM.lbPWM);
+	robot::setMotorPower(motorid_t::rearRightWheel, steer_PWM.rbPWM);
 }
 
 static bool validateJoint(const json& j) {

src/network/MissionControlTasks.cpp

@@ -86,35 +86,34 @@ void PowerRepeatTask::periodicTask() {
 					robot::setCmdVel(_last_cmd_vel->first, _last_cmd_vel->second);
 				}
 			} else if (swerveController.driveMode.first == DriveMode::TurnInPlace) {
-				std::vector<int> curr_wheel_rots = {
+				control::swerve_rots_t curr_wheel_rots = {
 					robot::getMotorPos(motorid_t::frontLeftWheel).getData(),
 					robot::getMotorPos(motorid_t::frontRightWheel).getData(),
 					robot::getMotorPos(motorid_t::rearLeftWheel).getData(),
 					robot::getMotorPos(motorid_t::rearRightWheel).getData()};
-				std::vector<double> new_wheel_rots =
+				control::swerve_commands_t steer_PWM =
 					Globals::swerveController
 						.setTurnInPlaceCmdVel(_last_cmd_vel->first, curr_wheel_rots)
 						.second;
-				robot::setMotorPower(motorid_t::frontLeftWheel, new_wheel_rots[0]);
-				robot::setMotorPower(motorid_t::frontRightWheel, new_wheel_rots[1]);
-				robot::setMotorPower(motorid_t::rearLeftWheel, new_wheel_rots[2]);
-				robot::setMotorPower(motorid_t::rearRightWheel, new_wheel_rots[3]);
+				robot::setMotorPower(motorid_t::frontLeftWheel, steer_PWM.lfPWM);
+				robot::setMotorPower(motorid_t::frontRightWheel, steer_PWM.rfPWM);
+				robot::setMotorPower(motorid_t::rearLeftWheel, steer_PWM.lbPWM);
+				robot::setMotorPower(motorid_t::rearRightWheel, steer_PWM.rbPWM);
 			} else {
-
-				std::vector<int> curr_wheel_rots = {
+				control::swerve_rots_t curr_wheel_rots = {
 					robot::getMotorPos(motorid_t::frontLeftWheel),
 					robot::getMotorPos(motorid_t::frontRightWheel),
 					robot::getMotorPos(motorid_t::rearLeftWheel),
 					robot::getMotorPos(motorid_t::rearRightWheel)};
-				std::vector<double> new_wheel_rots =
+				control::swerve_commands_t steer_PWM =
 					Globals::swerveController
 						.setCrabCmdVel(_last_cmd_vel->first, _last_cmd_vel->second,
 									   curr_wheel_rots)
 						.second;
-				robot::setMotorPower(motorid_t::frontLeftWheel, new_wheel_rots[0]);
-				robot::setMotorPower(motorid_t::frontRightWheel, new_wheel_rots[1]);
-				robot::setMotorPower(motorid_t::rearLeftWheel, new_wheel_rots[2]);
-				robot::setMotorPower(motorid_t::rearRightWheel, new_wheel_rots[3]);
+				robot::setMotorPower(motorid_t::frontLeftWheel, steer_PWM.lfPWM);
+				robot::setMotorPower(motorid_t::frontRightWheel, steer_PWM.rfPWM);
+				robot::setMotorPower(motorid_t::rearLeftWheel, steer_PWM.lbPWM);
+				robot::setMotorPower(motorid_t::rearRightWheel, steer_PWM.rbPWM);
 			}
 		}
 	}

src/world_interface/kinematic_common_interface.cpp

@@ -38,7 +38,7 @@ double setCmdVel(double dtheta, double dx) {
 		return 0;
 	}
 
-	std::vector<int> curr_wheel_rots = {
+	control::swerve_rots_t curr_wheel_rots = {
 		robot::getMotorPos(motorid_t::frontLeftWheel).getData(),
 		robot::getMotorPos(motorid_t::frontRightWheel).getData(),
 		robot::getMotorPos(motorid_t::rearLeftWheel).getData(),
@@ -69,7 +69,7 @@ double setTankCmdVel(double left, double right) {
 		return 0;
 	}
 
-	std::vector<int> curr_wheel_rots = {
+	control::swerve_rots_t curr_wheel_rots = {
 		robot::getMotorPos(motorid_t::frontLeftWheel).getData(),
 		robot::getMotorPos(motorid_t::frontRightWheel).getData(),
 		robot::getMotorPos(motorid_t::rearLeftWheel).getData(),