Fix bugs: convert rad to deg, reverse turn-in-place steering, etc. Mi…

…nor improvement to initMotors

src/control/SwerveController.cpp

@@ -25,12 +25,16 @@ drive_commands_t SwerveController::setTurnInPlaceCmdVel(double dtheta,
 		return {0.0, 0.0, 0.0, 0.0};
 	}
 
+	// robotVelToWheelVel() will return steers and vels such that the vels are positive. Since
+	// we're using preset steer angles this isn't correct, since we need to reverse the wheels
+	// to turn the other way instead of rotating the modules by 180deg. So, we just reverse the
+	// vels if the target rotVel is negative.
 	kinematics::swervewheelvel_t wheelVels =
-		swerve_kinematics.robotVelToWheelVel(0, 0, dtheta);
-	double lfPower = wheelVels.lfVel / Constants::MAX_WHEEL_VEL;
-	double lbPower = wheelVels.lbVel / Constants::MAX_WHEEL_VEL;
-	double rfPower = wheelVels.rfVel / Constants::MAX_WHEEL_VEL;
-	double rbPower = wheelVels.rbVel / Constants::MAX_WHEEL_VEL;
+		swerve_kinematics.robotVelToWheelVel(0, 0, std::abs(dtheta));
+	double lfPower = std::copysign(wheelVels.lfVel, dtheta) / Constants::MAX_WHEEL_VEL;
+	double lbPower = std::copysign(wheelVels.lbVel, dtheta) / Constants::MAX_WHEEL_VEL;
+	double rfPower = std::copysign(wheelVels.rfVel, dtheta) / Constants::MAX_WHEEL_VEL;
+	double rbPower = std::copysign(wheelVels.rbVel, dtheta) / Constants::MAX_WHEEL_VEL;
 	double maxAbsPWM = std::max(std::max(std::abs(lfPower), std::abs(lbPower)),
 								std::max(std::abs(rfPower), std::abs(rbPower)));
 
@@ -105,10 +109,11 @@ swerve_rots_t SwerveController::getSteerRots(DriveMode mode) const {
 		case DriveMode::TurnInPlace: {
 			kinematics::swervewheelvel_t wheelVels =
 				swerve_kinematics.robotVelToWheelVel(0, 0, 1);
-			return {static_cast<int32_t>(wheelVels.lfRot * 1000),
-					static_cast<int32_t>(wheelVels.rfRot * 1000),
-					static_cast<int32_t>(wheelVels.lbRot * 1000),
-					static_cast<int32_t>(wheelVels.rbRot * 1000)};
+			// convert from rad to deg
+			return {static_cast<int32_t>(wheelVels.lfRot * 1000 * 180 / M_PI),
+					static_cast<int32_t>(wheelVels.rfRot * 1000 * 180 / M_PI),
+					static_cast<int32_t>(wheelVels.lbRot * 1000 * 180 / M_PI),
+					static_cast<int32_t>(wheelVels.rbRot * 1000 * 180 / M_PI)};
 		}
 		case DriveMode::Crab:
 			return {90000, 90000, 90000, 90000};

src/network/MissionControlProtocol.cpp

@@ -177,7 +177,7 @@ void MissionControlProtocol::handleTankDriveRequest(const json& j) {
 }
 
 void MissionControlProtocol::handleTurnInPlaceDriveRequest(const json& j) {
-	double steer = j["steer"].get<double>();
+	double steer = -j["steer"].get<double>();
 	double dtheta = Constants::MAX_DTHETA * steer;
 	LOG_F(1, "{steer=%.2f} -> setTurnInPlaceCmdVel(%.4f)", steer, dtheta);
 	this->setRequestedTurnInPlaceCmdVel(dtheta);

src/world_interface/real_world_interface.cpp

@@ -66,6 +66,9 @@ std::unordered_map<callbackid_t, can::callbackid_t> callbackIDMap;
 void initMotors() {
 	for (const auto& it : motorSerialIDMap) {
 		can::motor::initMotor(it.second);
+		bool hasPosSensor = robot::potMotors.find(it.first) != robot::potMotors.end() ||
+							robot::encMotors.find(it.first) != robot::encMotors.end();
+		addMotorMapping(it.first, hasPosSensor);
 	}
 
 	for (const auto& pot_motor_pair : robot::potMotors) {
@@ -76,9 +79,6 @@ void initMotors() {
 
 		can::motor::initPotentiometer(serial, pot_params.mdeg_lo, pot_params.mdeg_hi,
 									  pot_params.adc_lo, pot_params.adc_hi, TELEM_PERIOD);
-
-		// initialize motor objects and add them to map
-		addMotorMapping(motor_id, true);
 	}
 
 	for (const auto& enc_motor_pair : robot::encMotors) {
@@ -91,17 +91,8 @@ void initMotors() {
 								TELEM_PERIOD);
 		can::motor::setLimitSwitchLimits(serial, enc_params.limitSwitchLow,
 										 enc_params.limitSwitchHigh);
-
-		// initialize motor objects and add them to map
-		addMotorMapping(motor_id, true);
 	}
 
-	// initialize motor objects and add them to map
-	addMotorMapping(motorid_t::frontLeftWheel, false);
-	addMotorMapping(motorid_t::frontRightWheel, false);
-	addMotorMapping(motorid_t::rearLeftWheel, false);
-	addMotorMapping(motorid_t::rearRightWheel, false);
-
 	for (const auto& pair : robot::motorPIDMap) {
 		motorid_t motor = pair.first;
 		can::deviceserial_t serial = motorSerialIDMap.at(motor);
@@ -110,7 +101,6 @@ void initMotors() {
 	}
 
 	can::motor::initMotor(motorSerialIDMap.at(motorid_t::hand));
-	addMotorMapping(motorid_t::hand, false);
 }
 
 void openCamera(CameraID camID, const char* cameraPath) {