From 8d6414be4d066cb0cba4a98cce33c9039fd45ab3 Mon Sep 17 00:00:00 2001 From: AyushKulk Date: Fri, 21 Jun 2024 00:23:23 -0700 Subject: [PATCH 1/6] Moved changes to the new control_interface --- src/Constants.h | 4 ++-- src/TunePID.cpp | 3 ++- src/control_interface.cpp | 6 ++++++ src/world_interface/data.cpp | 6 ++++-- src/world_interface/data.h | 13 ++++++++----- src/world_interface/real_world_constants.h | 16 +++++++++++----- src/world_interface/simulator_interface.cpp | 3 ++- 7 files changed, 35 insertions(+), 16 deletions(-) diff --git a/src/Constants.h b/src/Constants.h index da592f251..fb8f178f5 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -123,13 +123,13 @@ extern const std::unordered_map STREAM_RFS; /** * A map that pairs each of the joints to its corresponding motor. + * (one-to-one pairs only) */ -constexpr frozen::unordered_map +constexpr frozen::unordered_map JOINT_MOTOR_MAP{{robot::types::jointid_t::armBase, robot::types::motorid_t::armBase}, {robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder}, {robot::types::jointid_t::elbow, robot::types::motorid_t::elbow}, {robot::types::jointid_t::forearm, robot::types::motorid_t::forearm}, - {robot::types::jointid_t::wrist, robot::types::motorid_t::wrist}, {robot::types::jointid_t::hand, robot::types::motorid_t::hand}, {robot::types::jointid_t::activeSuspension, robot::types::motorid_t::activeSuspension}}; diff --git a/src/TunePID.cpp b/src/TunePID.cpp index 662b7a25b..972fdfce9 100644 --- a/src/TunePID.cpp +++ b/src/TunePID.cpp @@ -43,7 +43,8 @@ const std::map motorNameMap = { {motorid_t::shoulder, "shoulder"}, {motorid_t::elbow, "elbow"}, {motorid_t::forearm, "forearm"}, - {motorid_t::wrist, "wrist"}, + {motorid_t::wristDiffRight, "wristDiffRight"}, + {motorid_t::wristDiffLeft, "wristDiffLeft"}, {motorid_t::hand, "hand"}, {motorid_t::activeSuspension, "activeSuspension"}}; diff --git a/src/control_interface.cpp b/src/control_interface.cpp index c48299d05..e698fe2d5 100644 --- a/src/control_interface.cpp +++ b/src/control_interface.cpp @@ -164,6 +164,12 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) { } } } + } else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) { + kinematics::DiffWristKinematics diffWristKinematics; + setJointPowerValue(joint, power); + kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch), + getJointPowerValue(jointid_t::wristRoll)); + kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr); } else { LOG_F(WARNING, "setJointPower called for currently unsupported joint %s", util::to_string(joint).c_str()); diff --git a/src/world_interface/data.cpp b/src/world_interface/data.cpp index efc47856f..5a741e772 100644 --- a/src/world_interface/data.cpp +++ b/src/world_interface/data.cpp @@ -40,8 +40,10 @@ std::string to_string(robot::types::jointid_t joint) { return "forearm"; case jointid_t::hand: return "hand"; - case jointid_t::wrist: - return "wrist"; + case jointid_t::wristPitch: + return "wristPitch"; + case jointid_t::wristRoll: + return "wristRoll"; case jointid_t::activeSuspension: return "activeSuspension"; case jointid_t::ikUp: diff --git a/src/world_interface/data.h b/src/world_interface/data.h index 2024951e2..58830f361 100644 --- a/src/world_interface/data.h +++ b/src/world_interface/data.h @@ -67,7 +67,8 @@ enum class motorid_t { shoulder, elbow, forearm, - wrist, + wristDiffRight, + wristDiffLeft, hand, activeSuspension }; @@ -86,7 +87,8 @@ enum class jointid_t { shoulder, elbow, forearm, - wrist, + wristPitch, + wristRoll, hand, activeSuspension, ikForward, @@ -95,15 +97,16 @@ enum class jointid_t { constexpr auto all_jointid_t = frozen::make_unordered_set( {jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm, - jointid_t::wrist, jointid_t::hand, jointid_t::activeSuspension, jointid_t::ikForward, - jointid_t::ikUp}); + jointid_t::wristRoll, jointid_t::wristPitch, jointid_t::hand, jointid_t::activeSuspension, + jointid_t::ikForward, jointid_t::ikUp}); constexpr auto name_to_jointid = frozen::make_unordered_map( {{"armBase", jointid_t::armBase}, {"shoulder", jointid_t::shoulder}, {"elbow", jointid_t::elbow}, {"forearm", jointid_t::forearm}, - {"wrist", jointid_t::wrist}, + {"wristPitch", jointid_t::wristPitch}, + {"wristRoll", jointid_t::wristRoll}, {"hand", jointid_t::hand}, {"activeSuspension", jointid_t::activeSuspension}, {"ikForward", jointid_t::ikForward}, diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index b51a576a3..e24a986a9 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -79,8 +79,10 @@ constexpr auto potMotors = frozen::make_unordered_map({ {.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::wrist, - {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 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, @@ -107,7 +109,9 @@ constexpr auto motorSerialIDMap = frozen::make_unordered_map motorNameMap = { {motorid_t::shoulder, "shoulder"}, {motorid_t::elbow, "elbow"}, {motorid_t::forearm, "forearm"}, - {motorid_t::wrist, "wrist"}, + {motorid_t::wristDiffLeft, "wristDiffLeft"}, + {motorid_t::wristDiffRight, "wristDiffRight"}, {motorid_t::hand, "hand"}, {motorid_t::activeSuspension, "activeSuspension"}}; From 1a360d4242220d571803c2a7315c5ea18e3b5d21 Mon Sep 17 00:00:00 2001 From: AyushKulk Date: Fri, 21 Jun 2024 00:31:14 -0700 Subject: [PATCH 2/6] white space --- src/control_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control_interface.cpp b/src/control_interface.cpp index e698fe2d5..0ee4dd82c 100644 --- a/src/control_interface.cpp +++ b/src/control_interface.cpp @@ -169,7 +169,7 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) { setJointPowerValue(joint, power); kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch), getJointPowerValue(jointid_t::wristRoll)); - kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr); + kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr); } else { LOG_F(WARNING, "setJointPower called for currently unsupported joint %s", util::to_string(joint).c_str()); From 8309223263e95e0f7aac710de8f72d4a2f8acecc Mon Sep 17 00:00:00 2001 From: Abhay D Date: Fri, 28 Jun 2024 00:16:51 -0700 Subject: [PATCH 3/6] Finish wrist implementation, fix bugs, move wrist kinematics to globals --- src/Globals.cpp | 1 + src/Globals.h | 2 + src/control_interface.cpp | 43 +++++++++----------- src/world_interface/noop_world.cpp | 6 --- src/world_interface/real_world_interface.cpp | 5 --- src/world_interface/simulator_interface.cpp | 5 --- src/world_interface/world_interface.h | 2 - 7 files changed, 22 insertions(+), 42 deletions(-) diff --git a/src/Globals.cpp b/src/Globals.cpp index 40f5ae0ad..04ddb6325 100644 --- a/src/Globals.cpp +++ b/src/Globals.cpp @@ -46,6 +46,7 @@ net::websocket::SingleClientWSServer websocketServer("DefaultServer", Constants::WS_SERVER_PORT); std::atomic AUTONOMOUS = false; robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperipheral_t::none; +const kinematics::DiffWristKinematics wristKinematics; control::PlanarArmController<2> planarArmController(createArmKinematics(), Constants::arm::SAFETY_FACTOR); std::atomic armIKEnabled = false; diff --git a/src/Globals.h b/src/Globals.h index 6ad315fff..0b4724dbf 100644 --- a/src/Globals.h +++ b/src/Globals.h @@ -2,6 +2,7 @@ #include "control/PlanarArmController.h" #include "kinematics/ArmKinematics.h" +#include "kinematics/DiffWristKinematics.h" #include "network/websocket/WebSocketServer.h" #include "world_interface/data.h" @@ -28,5 +29,6 @@ extern net::websocket::SingleClientWSServer websocketServer; extern std::atomic AUTONOMOUS; extern robot::types::mountedperipheral_t mountedPeripheral; extern control::PlanarArmController<2> planarArmController; +extern const kinematics::DiffWristKinematics wristKinematics; extern std::atomic armIKEnabled; } // namespace Globals diff --git a/src/control_interface.cpp b/src/control_interface.cpp index 0ee4dd82c..a4f8bd536 100644 --- a/src/control_interface.cpp +++ b/src/control_interface.cpp @@ -71,27 +71,7 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) { using robot::types::jointid_t; if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) { setMotorPos(Constants::JOINT_MOTOR_MAP.at(joint), targetPos); - } - // FIXME: need to do some extra control (probably implementing our own PID control) for the - // differential position, since the potentiometers are giving us joint position instead of - // motor position. - /* - else if (joint == jointid_t::differentialPitch || joint == jointid_t::differentialRoll) { - std::lock_guard lk(wristPosMutex); - if (joint == jointid_t::differentialPitch){ - commandedWristPos.pitch = targetPos; - } else { - commandedWristPos.roll = targetPos; - } - gearpos_t gearPos = wristKinematics().jointPosToGearPos(commandedWristPos); - setMotorPos(motorid_t::differentialLeft, gearPos.left); - setMotorPos(motorid_t::differentialRight, gearPos.right); - } - */ - else { - // FIXME: this should ideally never happen, but we don't have support for all joints - // yet because we don't know anything about the drill arm (and need to do extra work - // for the differential) + } else { LOG_F(WARNING, "setJointPos called for currently unsupported joint %s", util::to_string(joint).c_str()); } @@ -113,6 +93,19 @@ types::DataPoint getJointPos(robot::types::jointid_t joint) { } else { return {}; } + } else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) { + auto mdegToRad = [](int32_t mdeg) { return (mdeg / 1000.0) * (M_PI / 180.0); }; + auto lPos = getMotorPos(motorid_t::wristDiffLeft).transform(mdegToRad); + auto rPos = getMotorPos(motorid_t::wristDiffRight).transform(mdegToRad); + if (lPos.isValid() && rPos.isValid()) { + kinematics::gearpos_t gearPos(lPos.getData(), rPos.getData()); + auto jointPos = Globals::wristKinematics.gearPosToJointPos(gearPos); + float angle = joint == jointid_t::wristPitch ? jointPos.pitch : jointPos.roll; + return DataPoint(lPos.getTime(), + static_cast(angle * (180.0 / M_PI) * 1000)); + } else { + return {}; + } } else { // This should ideally never happen, but may if we haven't implemented a joint yet. LOG_F(WARNING, "getJointPos called for currently unsupported joint %s", @@ -165,11 +158,13 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) { } } } else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) { - kinematics::DiffWristKinematics diffWristKinematics; setJointPowerValue(joint, power); kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch), - getJointPowerValue(jointid_t::wristRoll)); - kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr); + getJointPowerValue(jointid_t::wristRoll)); + kinematics::gearpos_t gearPwr = + Globals::wristKinematics.jointPowerToGearPower(jointPwr); + setMotorPower(motorid_t::wristDiffLeft, gearPwr.left); + setMotorPower(motorid_t::wristDiffRight, gearPwr.right); } else { LOG_F(WARNING, "setJointPower called for currently unsupported joint %s", util::to_string(joint).c_str()); diff --git a/src/world_interface/noop_world.cpp b/src/world_interface/noop_world.cpp index 0f100e54d..879ebbffa 100644 --- a/src/world_interface/noop_world.cpp +++ b/src/world_interface/noop_world.cpp @@ -6,7 +6,6 @@ // using namespace kinematics; using kinematics::DiffDriveKinematics; -using kinematics::DiffWristKinematics; using namespace navtypes; using namespace robot::types; @@ -16,17 +15,12 @@ extern const WorldInterface WORLD_INTERFACE = WorldInterface::noop; namespace { DiffDriveKinematics drive_kinematics(1); // doesn't really matter what we set this to -DiffWristKinematics wrist_kinematics; } // namespace const DiffDriveKinematics& driveKinematics() { return drive_kinematics; } -const DiffWristKinematics& wristKinematics() { - return wrist_kinematics; -} - void world_interface_init( std::optional> wsServer, bool initMotorsOnly) {} diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index bfc44fa29..a15a3f0bd 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -33,7 +33,6 @@ std::unordered_map> namespace { kinematics::DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE); -kinematics::DiffWristKinematics wrist_kinematics; bool is_emergency_stopped = false; void addMotorMapping(motorid_t motor, bool hasPosSensor) { @@ -52,10 +51,6 @@ const kinematics::DiffDriveKinematics& driveKinematics() { return drive_kinematics; } -const kinematics::DiffWristKinematics& wristKinematics() { - return wrist_kinematics; -} - namespace { // map that associates camera id to the camera object std::unordered_map> cameraMap; diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index d66d2e78a..3d0b5f7b5 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -247,17 +247,12 @@ namespace robot { namespace { DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE); -DiffWristKinematics wrist_kinematics; } // namespace const DiffDriveKinematics& driveKinematics() { return drive_kinematics; } -const DiffWristKinematics& wristKinematics() { - return wrist_kinematics; -} - extern const WorldInterface WORLD_INTERFACE = WorldInterface::sim3d; void world_interface_init( diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index b48e06bd1..e96ba99bc 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -2,7 +2,6 @@ #include "../gps/gps_util.h" #include "../kinematics/DiffDriveKinematics.h" -#include "../kinematics/DiffWristKinematics.h" #include "../navtypes.h" #include "../network/websocket/WebSocketServer.h" #include "data.h" @@ -37,7 +36,6 @@ extern const WorldInterface WORLD_INTERFACE; // TODO: add documentation const kinematics::DiffDriveKinematics& driveKinematics(); -const kinematics::DiffWristKinematics& wristKinematics(); /** * @brief Initialize the world interface. From a25d147710eaab88e623881689a909ee6a590e7e Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Mon, 8 Jul 2024 16:03:15 -0700 Subject: [PATCH 4/6] Some tuning --- src/CMakeLists.txt | 2 +- src/world_interface/real_world_constants.h | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 524a5695d..b55de633b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -94,7 +94,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED) FetchContent_Declare( HindsightCAN GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git - GIT_TAG 7397787fbb04687bf03c8fbac9a1db56f245fc9a + GIT_TAG 45b1544f20e7ba6dd0db19fb97e947430031fc6a ) FetchContent_MakeAvailable(HindsightCAN) diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index efacd456e..0aa959e48 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -109,9 +109,8 @@ constexpr auto motorSerialIDMap = frozen::make_unordered_map Date: Wed, 17 Jul 2024 16:00:19 -0700 Subject: [PATCH 5/6] Various tuning and fixes --- rover-config/50-rover-cameras.rules | 8 ++--- src/CAN/CANMotor.cpp | 7 ++-- src/control/SwerveController.cpp | 4 +++ src/control/SwerveController.h | 7 ++++ src/control_interface.cpp | 26 ++++++++------ src/world_interface/real_world_constants.h | 38 ++++++++------------ src/world_interface/real_world_interface.cpp | 2 -- 7 files changed, 49 insertions(+), 43 deletions(-) diff --git a/rover-config/50-rover-cameras.rules b/rover-config/50-rover-cameras.rules index e4d7242fe..255c4ac95 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 6d8563e77..a68eda07c 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 2e1b2fb8c..a4cb31c1e 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 c579eaa2f..a94b3e0c2 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 1ac4090ce..79c912422 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 0aa959e48..f74605c58 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 aa71e3145..814442f0e 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) { From a691d2b5e844e853f758cf39a1c7e486c13a746d Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Wed, 17 Jul 2024 16:11:08 -0700 Subject: [PATCH 6/6] Fix formatting --- src/control/SwerveController.h | 2 +- src/world_interface/real_world_constants.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/control/SwerveController.h b/src/control/SwerveController.h index a94b3e0c2..20e2997bd 100644 --- a/src/control/SwerveController.h +++ b/src/control/SwerveController.h @@ -138,7 +138,7 @@ class SwerveController { * @brief Check the override flag for wheel rotation checking. * * @return True iff the rotation checking is overridden. - */ + */ bool isOverridden() const; /** diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index f74605c58..e23b08a5c 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -105,9 +105,8 @@ 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::frontLeftSwerve, {2, 0, 0}}, {motorid_t::frontRightSwerve, {2, 0, 0}}, {motorid_t::rearLeftSwerve, {2, 0, 0}},