Various tuning and fixes

rover-config/50-rover-cameras.rules

@@ -6,11 +6,11 @@
 #
 # Install this in /etc/udev/rules.d
 
-# Mast camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video20"
+# Hand camera
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="6369", KERNELS=="1-2.1", SYMLINK+="video20"
 
 # Forearm camera
-# SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video30"
 
-# Hand camera
+# Mast camera
 SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.4.4", SYMLINK+="video40"

src/CAN/CANMotor.cpp

@@ -44,6 +44,7 @@ void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint
 	auto group = static_cast<uint8_t>(devicegroup_t::motor);
 	AssemblePotHiSetPacket(&packet, group, serial, adcHi, posHi);
 	sendCANPacket(packet);
+	std::this_thread::sleep_for(1ms);
 	AssemblePotLoSetPacket(&packet, group, serial, adcLo, posLo);
 	sendCANPacket(packet);
 	if (telemetryPeriod) {
@@ -72,7 +73,7 @@ void setLimitSwitchLimits(deviceserial_t serial, int32_t lo, int32_t hi) {
 	// 0 is low limit, 1 is high limit.
 	AssembleLimSwEncoderBoundPacket(&p, motorGroupCode, serial, 0, lo);
 	sendCANPacket(p);
-
+	std::this_thread::sleep_for(1ms);
 	AssembleLimSwEncoderBoundPacket(&p, motorGroupCode, serial, 1, hi);
 	sendCANPacket(p);
 }
@@ -82,11 +83,13 @@ void setMotorPIDConstants(deviceserial_t serial, int32_t kP, int32_t kI, int32_t
 	auto motorGroupCode = static_cast<uint8_t>(devicegroup_t::motor);
 	AssemblePSetPacket(&p, motorGroupCode, serial, kP);
 	sendCANPacket(p);
+	std::this_thread::sleep_for(1ms);
 	AssembleISetPacket(&p, motorGroupCode, serial, kI);
 	sendCANPacket(p);
+	std::this_thread::sleep_for(1ms);
 	AssembleDSetPacket(&p, motorGroupCode, serial, kD);
 	sendCANPacket(p);
-	std::this_thread::sleep_for(1000us);
+	std::this_thread::sleep_for(1ms);
 }
 
 void setMotorMode(deviceserial_t serial, motormode_t mode) {

src/control/SwerveController.cpp

@@ -138,6 +138,10 @@ void SwerveController::setOverride(bool override) {
 	override_steer_check = override;
 }
 
+bool SwerveController::isOverridden() const {
+	return override_steer_check;
+}
+
 namespace util {
 std::string to_string(control::DriveMode mode) {
 	using control::DriveMode;

src/control/SwerveController.h

@@ -134,6 +134,13 @@ class SwerveController {
 	 */
 	void setOverride(bool override);
 
+	/**
+	 * @brief Check the override flag for wheel rotation checking.
+	 *
+	 * @return True iff the rotation checking is overridden.
+	*/
+	bool isOverridden() const;
+
 	/**
 	 * @brief Get the current drive mode of the controller.
 	 *

src/control_interface.cpp

@@ -40,18 +40,22 @@ double setCmdVel(double dtheta, double dx) {
 		return 0;
 	}
 
-	control::swerve_rots_t curr_wheel_rots;
-	try {
-		curr_wheel_rots = {robot::getMotorPos(motorid_t::frontLeftSwerve).getData(),
-						   robot::getMotorPos(motorid_t::frontRightSwerve).getData(),
-						   robot::getMotorPos(motorid_t::rearLeftSwerve).getData(),
-						   robot::getMotorPos(motorid_t::rearRightSwerve).getData()};
-	} catch (const bad_datapoint_access& e) {
-		LOG_F(WARNING, "Invalid steer motor position(s)!");
-		return 0;
+	if (!Globals::swerveController.isOverridden()) {
+		try {
+			control::swerve_rots_t curr_wheel_rots = {
+				robot::getMotorPos(motorid_t::frontLeftSwerve).getData(),
+				robot::getMotorPos(motorid_t::frontRightSwerve).getData(),
+				robot::getMotorPos(motorid_t::rearLeftSwerve).getData(),
+				robot::getMotorPos(motorid_t::rearRightSwerve).getData()};
+			if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal,
+															  curr_wheel_rots)) {
+				return 0;
+			}
+		} catch (const bad_datapoint_access& e) {
+			LOG_F(WARNING, "Invalid steer motor position(s)!");
+			return 0;
+		}
 	}
-	if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal, curr_wheel_rots))
-		return 0;
 
 	wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta);
 	double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL;

src/world_interface/real_world_constants.h

@@ -63,28 +63,18 @@ constexpr auto encMotors = frozen::make_unordered_map<motorid_t, encparams_t>({
 		.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,
-		.zeroCalibrationPower = 0.4}},
-	{motorid_t::elbow,
-		{.isInverted = false,
-		.ppjr = 1620 * 1024 * 4,
-		.limitSwitchLow = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::elbow).first,
-		.limitSwitchHigh = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::elbow).second,
-		.zeroCalibrationPower = -0.15}}
+		.zeroCalibrationPower = 0.4}}
 });
 // clang-format on
 
 // TODO: find appropriate bounds
 constexpr auto potMotors = frozen::make_unordered_map<motorid_t, potparams_t>({
-	{motorid_t::armBase,
-	 {.adc_lo = 123, .mdeg_lo = -200 * 1000, .adc_hi = 456, .mdeg_hi = 200 * 1000}},
 	{motorid_t::forearm,
 	 {.adc_lo = 1208, .mdeg_lo = -180 * 1000, .adc_hi = 841, .mdeg_hi = 180 * 1000}},
 	{motorid_t::wristDiffLeft,
 	 {.adc_lo = 0, .mdeg_lo = -100 * 0, .adc_hi = 0, .mdeg_hi = 100 * 0}},
 	{motorid_t::wristDiffRight,
 	 {.adc_lo = 0, .mdeg_lo = -100 * 0, .adc_hi = 0, .mdeg_hi = 100 * 0}},
-	{motorid_t::activeSuspension,
-	 {.adc_lo = 251, .mdeg_lo = -19 * 1000, .adc_hi = 1645, .mdeg_hi = 31 * 1000}},
 	{motorid_t::frontLeftSwerve,
 	 {.adc_lo = 1422, .mdeg_lo = -180 * 1000, .adc_hi = 656, .mdeg_hi = 180 * 1000}},
 	{motorid_t::frontRightSwerve,
@@ -115,9 +105,9 @@ constexpr auto motorSerialIDMap = frozen::make_unordered_map<motorid_t, can::dev
 	 {motorid_t::activeSuspension, DEVICE_SERIAL_LINEAR_ACTUATOR}});
 
 /** @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::elbow, {15, 7, -2}},
+constexpr auto motorPIDMap = frozen::make_unordered_map<motorid_t, pidcoef_t>({
+	// {{motorid_t::shoulder, {70, 0, 0}},
+	//  {motorid_t::elbow, {15, 7, -2}},
 	 {motorid_t::frontLeftSwerve, {2, 0, 0}},
 	 {motorid_t::frontRightSwerve, {2, 0, 0}},
 	 {motorid_t::rearLeftSwerve, {2, 0, 0}},
@@ -128,16 +118,16 @@ constexpr auto motorPIDMap = frozen::make_unordered_map<motorid_t, pidcoef_t>(
  * Negative values mean that the motor is inverted.
  */
 constexpr auto positive_pwm_scales =
-	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.75},
-												   {motorid_t::shoulder, 1},
-												   {motorid_t::elbow, 1},
-												   {motorid_t::forearm, -0.65},
+	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.25},
+												   {motorid_t::shoulder, -1},
+												   {motorid_t::elbow, -1},
+												   {motorid_t::forearm, -0.2},
 												   {motorid_t::wristDiffLeft, -0.1},
 												   {motorid_t::wristDiffRight, 0.1},
 												   {motorid_t::frontLeftWheel, 0.7},
 												   {motorid_t::frontRightWheel, 0.7},
 												   {motorid_t::rearLeftWheel, 0.7},
-												   {motorid_t::rearRightWheel, -0.7},
+												   {motorid_t::rearRightWheel, 0.7},
 												   {motorid_t::frontLeftSwerve, 1.0},
 												   {motorid_t::frontRightSwerve, 1.0},
 												   {motorid_t::rearLeftSwerve, 1.0},
@@ -149,16 +139,16 @@ constexpr auto positive_pwm_scales =
  * Negative values mean that the motor is inverted.
  */
 constexpr auto negative_pwm_scales =
-	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.75},
-												   {motorid_t::shoulder, 1},
-												   {motorid_t::elbow, 1},
-												   {motorid_t::forearm, -0.65},
+	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, -0.25},
+												   {motorid_t::shoulder, -1},
+												   {motorid_t::elbow, -1},
+												   {motorid_t::forearm, -0.2},
 												   {motorid_t::wristDiffLeft, -0.1},
 												   {motorid_t::wristDiffRight, 0.1},
 												   {motorid_t::frontLeftWheel, 0.7},
 												   {motorid_t::frontRightWheel, 0.7},
 												   {motorid_t::rearLeftWheel, 0.7},
-												   {motorid_t::rearRightWheel, -0.7},
+												   {motorid_t::rearRightWheel, 0.7},
 												   {motorid_t::frontLeftSwerve, 1.0},
 												   {motorid_t::frontRightSwerve, 1.0},
 												   {motorid_t::rearLeftSwerve, 1.0},

src/world_interface/real_world_interface.cpp

@@ -94,8 +94,6 @@ void initMotors() {
 		pidcoef_t pid = motorPIDMap.at(motor);
 		can::motor::setMotorPIDConstants(serial, pid.kP, pid.kI, pid.kD);
 	}
-
-	can::motor::initMotor(motorSerialIDMap.at(motorid_t::hand));
 }
 
 void openCamera(CameraID camID, const char* cameraPath) {