diff --git a/src/Globals.cpp b/src/Globals.cpp index 40f5ae0a..04ddb632 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 6ad315ff..0b4724db 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 0ee4dd82..a4f8bd53 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 0f100e54..879ebbff 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 bfc44fa2..a15a3f0b 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 d66d2e78..3d0b5f7b 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 b48e06bd..e96ba99b 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.