Some of the IK Fixes From CIRC

src/CAN/CANMotor.cpp

@@ -92,6 +92,13 @@ void setMotorPIDConstants(deviceserial_t serial, int32_t kP, int32_t kI, int32_t
 	std::this_thread::sleep_for(1ms);
 }
 
+void setMotorPIDMaxPower(deviceserial_t serial, uint16_t maxPower) {
+	CANPacket p;
+	auto motorGroupCode = static_cast<uint8_t>(devicegroup_t::motor);
+	AssembleMaxPIDPWMPacket(&p, motorGroupCode, serial, maxPower);
+	sendCANPacket(p);
+}
+
 void setMotorMode(deviceserial_t serial, motormode_t mode) {
 	CANPacket p;
 	AssembleModeSetPacket(&p, static_cast<uint8_t>(devicegroup_t::motor), serial,

src/CAN/CANMotor.h

@@ -16,11 +16,17 @@
 namespace can::motor {
 
 /** @brief The possible motor modes. */
-enum class motormode_t { pwm = MOTOR_UNIT_MODE_PWM, pid = MOTOR_UNIT_MODE_PID };
+enum class motormode_t {
+	pwm = MOTOR_UNIT_MODE_PWM,
+	pid = MOTOR_UNIT_MODE_PID
+};
 
 /** @brief The supported motor position sensors. */
 struct sensor_t {
-	enum { encoder = 0, potentiometer = 1 };
+	enum {
+		encoder = 0,
+		potentiometer = 1
+	};
 };
 
 /**
@@ -65,8 +71,8 @@ void initEncoder(deviceserial_t serial, bool invertEncoder, bool zeroEncoder,
  * Use this method for motorboard with both encoders and limit switches
  *
  * @param serial The CAN serial number of the motor board.
- * @param lo The joint position in millidegrees of the low limit switch.
- * @param hi The joint position in millidegrees of the high limit switch.
+ * @param lo The joint position in encoder ticks of the low limit switch.
+ * @param hi The joint position in encoder ticks of the high limit switch.
  */
 void setLimitSwitchLimits(deviceserial_t serial, int32_t lo, int32_t hi);
 
@@ -89,8 +95,8 @@ void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint
 /**
  * @brief Set the PID constants for a motor board.
  *
- * Note that the PID constants are specified in units of 10000, so a 1 is interpreted as a
- * 10000. This is because the controller operates on millidegrees.
+ * Note that the PID constants are specified in units of 1000, so a 1 is interpreted as a
+ * 1000. This is because the controller operates on millidegrees.
  *
  * @param serial The CAN serial number of the motor board.
  * @param kP The P coefficient.
@@ -136,6 +142,14 @@ void setMotorPower(deviceserial_t serial, int16_t power);
  */
 void setMotorPIDTarget(deviceserial_t serial, int32_t target);
 
+/**
+ * @brief Set the maximum power allowed for a motor when running PID.
+ *
+ * @param serial The CAN serial number of the motor board.
+ * @param maxPower The maximum power allowed for the motor.
+ */
+void setMotorPIDMaxPower(deviceserial_t serial, uint16_t maxPower);
+
 /**
  * @brief Set the angle of the PCA servo
  *

src/Constants.h

@@ -186,8 +186,8 @@ constexpr frozen::unordered_map<robot::types::motorid_t, std::pair<int, int>, IK
  * Map from motor ids to segment length in meters
  */
 constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size()>
-	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608},
-					{robot::types::motorid_t::elbow, 0.461264}};
+	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.394},
+					{robot::types::motorid_t::elbow, 0.749}};
 } // namespace arm
 
 namespace autonomous {

src/TunePID.cpp

@@ -77,8 +77,13 @@ bool initMotor(motorid_t motor) {
 		robot::encparams_t enc_params = robot::encMotors.at(motor);
 		can::motor::initEncoder(serial, enc_params.isInverted, true, enc_params.ppjr,
 								robot::TELEM_PERIOD);
-		can::motor::setLimitSwitchLimits(serial, enc_params.limitSwitchLow,
-										 enc_params.limitSwitchHigh);
+
+		auto mDegToPulses = [&](int32_t mdeg) {
+			return static_cast<int32_t>(static_cast<double>(mdeg) / 360000.0 *
+										enc_params.ppjr);
+		};
+		can::motor::setLimitSwitchLimits(serial, mDegToPulses(enc_params.limitSwitchLow),
+										 mDegToPulses(enc_params.limitSwitchHigh));
 	} else {
 		std::cerr << "The given motor is not a pot or enc motor!\n";
 		return false;
@@ -133,13 +138,13 @@ int main(int argc, char** argv) {
 	std::getline(std::cin, str);
 	int d_coeff = std::stoi(str);
 
-	can::motor::setMotorMode(serial, can::motor::motormode_t::pid);
 	can::motor::setMotorPIDConstants(serial, p_coeff, i_coeff, d_coeff);
-
+	can::motor::setMotorMode(serial, can::motor::motormode_t::pid);
 	double period = 8.0;
 
+	// TODO: remove and replace with a loop
 	std::this_thread::sleep_for(300ms); // wait for encoder position data to arrive
-	int32_t starting_angle = can::motor::getMotorPosition(serial);
+	int32_t starting_angle = can::motor::getMotorPosition(serial).getData();
 	int32_t angle_target = starting_angle;
 	double acc_error = 0.0;
 	int total_steps = 0;
@@ -161,7 +166,8 @@ int main(int argc, char** argv) {
 		if (mode == targetmode_t::step) {
 			prescaled_target = round(prescaled_target);
 		}
-		angle_target = (int32_t)round(amplitude * prescaled_target) + starting_angle;
+		angle_target =
+			static_cast<int32_t>(round(amplitude * prescaled_target)) + starting_angle;
 
 		can::motor::setMotorPIDTarget(serial, angle_target);
 

src/utils/core.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <algorithm>
 #include <functional>
 #include <string>
 #include <tuple>

src/world_interface/real_world_constants.h

@@ -59,7 +59,7 @@ struct encparams_t {
 // clang-format off
 constexpr auto encMotors = frozen::make_unordered_map<motorid_t, encparams_t>({
 	{motorid_t::shoulder,
-		{.isInverted = true,
+		{.isInverted = false,
 		.ppjr = 4590 * 1024 * 4,
 		.limitSwitchLow = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::shoulder).first,
 		.limitSwitchHigh = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::shoulder).second,
@@ -69,6 +69,7 @@ constexpr auto encMotors = frozen::make_unordered_map<motorid_t, encparams_t>({
 
 // TODO: find appropriate bounds
 constexpr auto potMotors = frozen::make_unordered_map<motorid_t, potparams_t>({
+	{motorid_t::elbow, {.adc_lo = 563, .mdeg_lo = -160000, .adc_hi = 1927, .mdeg_hi = 0}},
 	{motorid_t::forearm,
 	 {.adc_lo = 1208, .mdeg_lo = -180 * 1000, .adc_hi = 841, .mdeg_hi = 180 * 1000}},
 	{motorid_t::wristDiffLeft,
@@ -106,7 +107,10 @@ constexpr auto motorSerialIDMap = frozen::make_unordered_map<motorid_t, can::dev
 
 /** @brief A mapping of PID controlled motors to their pid coefficients. */
 constexpr auto motorPIDMap = frozen::make_unordered_map<motorid_t, pidcoef_t>(
-	{{motorid_t::shoulder, {70, 0, 0}},
+	{{motorid_t::shoulder, {200, 0, 0}},
+	 {motorid_t::elbow, {250, 0, 0}},
+	 // FIXME: swerve constants need to be updated when firmware changes PID scaling
+	 // Numbers should be 200
 	 {motorid_t::frontLeftSwerve, {2, 0, 0}},
 	 {motorid_t::frontRightSwerve, {2, 0, 0}},
 	 {motorid_t::rearLeftSwerve, {2, 0, 0}},
@@ -118,8 +122,8 @@ constexpr auto motorPIDMap = frozen::make_unordered_map<motorid_t, pidcoef_t>(
  */
 constexpr auto positive_pwm_scales =
 	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.25},
-												   {motorid_t::shoulder, -1},
-												   {motorid_t::elbow, -1},
+												   {motorid_t::shoulder, 1},
+												   {motorid_t::elbow, 1},
 												   {motorid_t::forearm, -0.2},
 												   {motorid_t::wristDiffLeft, -0.1},
 												   {motorid_t::wristDiffRight, 0.1},
@@ -139,8 +143,8 @@ constexpr auto positive_pwm_scales =
  */
 constexpr auto negative_pwm_scales =
 	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.25},
-												   {motorid_t::shoulder, -1},
-												   {motorid_t::elbow, -1},
+												   {motorid_t::shoulder, 1},
+												   {motorid_t::elbow, 1},
 												   {motorid_t::forearm, -0.2},
 												   {motorid_t::wristDiffLeft, -0.1},
 												   {motorid_t::wristDiffRight, 0.1},

src/world_interface/real_world_interface.cpp

@@ -84,8 +84,15 @@ void initMotors() {
 
 		can::motor::initEncoder(serial, enc_params.isInverted, true, enc_params.ppjr,
 								TELEM_PERIOD);
-		can::motor::setLimitSwitchLimits(serial, enc_params.limitSwitchLow,
-										 enc_params.limitSwitchHigh);
+
+		auto mDegToPulses = [&](int32_t mdeg) {
+			return static_cast<int32_t>(static_cast<double>(mdeg) / 360000.0 *
+										enc_params.ppjr);
+		};
+
+		// TODO: get rid of the negatives once firmware fixes the limit switch limits issue
+		can::motor::setLimitSwitchLimits(serial, mDegToPulses(-enc_params.limitSwitchLow),
+										 mDegToPulses(-enc_params.limitSwitchHigh));
 	}
 
 	for (const auto& pair : robot::motorPIDMap) {