diff --git a/rover-config/50-rover-cameras.rules b/rover-config/50-rover-cameras.rules index e4d7242f..255c4ac9 100644 --- a/rover-config/50-rover-cameras.rules +++ b/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" diff --git a/src/CAN/CANMotor.cpp b/src/CAN/CANMotor.cpp index 6d8563e7..a68eda07 100644 --- a/src/CAN/CANMotor.cpp +++ b/src/CAN/CANMotor.cpp @@ -44,6 +44,7 @@ void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint auto group = static_cast(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(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) { diff --git a/src/control/SwerveController.cpp b/src/control/SwerveController.cpp index 2e1b2fb8..a4cb31c1 100644 --- a/src/control/SwerveController.cpp +++ b/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; diff --git a/src/control/SwerveController.h b/src/control/SwerveController.h index c579eaa2..a94b3e0c 100644 --- a/src/control/SwerveController.h +++ b/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. * diff --git a/src/control_interface.cpp b/src/control_interface.cpp index 1ac4090c..79c91242 100644 --- a/src/control_interface.cpp +++ b/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; diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index 0aa959e4..f74605c5 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -63,28 +63,18 @@ constexpr auto encMotors = frozen::make_unordered_map({ .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::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::shoulder, {70, 0, 0}}, - {motorid_t::elbow, {15, 7, -2}}, +constexpr auto motorPIDMap = frozen::make_unordered_map({ + // {{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( * Negative values mean that the motor is inverted. */ constexpr auto positive_pwm_scales = - frozen::make_unordered_map({{motorid_t::armBase, -0.75}, - {motorid_t::shoulder, 1}, - {motorid_t::elbow, 1}, - {motorid_t::forearm, -0.65}, + frozen::make_unordered_map({{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::armBase, -0.75}, - {motorid_t::shoulder, 1}, - {motorid_t::elbow, 1}, - {motorid_t::forearm, -0.65}, + frozen::make_unordered_map({{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}, diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index aa71e314..814442f0 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/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) {