From e04adf4ba0ea15aec223110a1c010c163a454064 Mon Sep 17 00:00:00 2001 From: suhas Date: Fri, 11 Apr 2025 22:01:26 -0500 Subject: [PATCH 01/19] write an echo thing? --- ut-robomaster/src/communication/cv_board.cpp | 18 ++++++++++++++++++ ut-robomaster/src/communication/cv_board.hpp | 5 +++++ ut-robomaster/src/communication/cv_message.hpp | 3 +++ 3 files changed, 26 insertions(+) diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index db545381..da7dfa31 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -24,6 +24,8 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message) case CV_MESSAGE_TYPE_TURRET_AIM: decodeTurretData(message); break; + case CV_MESSAGE_TYPE_ECHO: + echoData(message); default: break; } @@ -35,6 +37,22 @@ void CVBoard::sendMessage() sendColorData(); } +void CVBoard::echoData(const ReceivedSerialMessage& message) +{ + std::string data_string; + memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); + + DJISerial::SerialMessage msg; + msg.messageType = CV_MESSAGE_TYPE_ECHO; + + std::string* data = reinterpret_cast(msg.data); + + data = &data_string; + + msg.setCRC16(); + drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); +} + void CVBoard::sendOdometryData() { DJISerial::SerialMessage message; diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp index 89e00d0e..6590e56c 100644 --- a/ut-robomaster/src/communication/cv_board.hpp +++ b/ut-robomaster/src/communication/cv_board.hpp @@ -43,6 +43,11 @@ class CVBoard : public tap::communication::serial::DJISerial */ void sendMessage(); + /** + * Echo the message sent by the CV board to the CV board + */ + void echoData(const ReceivedSerialMessage& message); + /** * Sends odometry data to the CV board */ diff --git a/ut-robomaster/src/communication/cv_message.hpp b/ut-robomaster/src/communication/cv_message.hpp index 263164df..5899757a 100644 --- a/ut-robomaster/src/communication/cv_message.hpp +++ b/ut-robomaster/src/communication/cv_message.hpp @@ -1,6 +1,8 @@ #pragma once #include +#include + namespace communication { @@ -9,6 +11,7 @@ enum MessageTypes : uint8_t CV_MESSAGE_TYPE_ODOMETRY_DATA = 1, CV_MESSAGE_TYPE_TURRET_AIM = 2, CV_MESSAGE_TYPE_COLOR_DATA = 3, + CV_MESSAGE_TYPE_ECHO = 4, }; enum ColorTypes : uint8_t From e1f2ac09f7e17f08d71ef8004432fb23effd8fdc Mon Sep 17 00:00:00 2001 From: suhas Date: Wed, 16 Apr 2025 21:54:28 +0000 Subject: [PATCH 02/19] OMNI WHEEL CODE !!!!!! Co-authored-by: arekCodez --- ut-robomaster/src/communication/cv_board.cpp | 26 +++++----- ut-robomaster/src/communication/cv_board.hpp | 2 +- .../subsystems/chassis/chassis_subsystem.cpp | 8 ++++ .../subsystems/chassis/chassis_subsystem.hpp | 2 + .../chassis/command_sentry_position.cpp | 47 +++++++++++++++---- 5 files changed, 63 insertions(+), 22 deletions(-) diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index da7dfa31..4de325cd 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -24,8 +24,8 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message) case CV_MESSAGE_TYPE_TURRET_AIM: decodeTurretData(message); break; - case CV_MESSAGE_TYPE_ECHO: - echoData(message); + // case CV_MESSAGE_TYPE_ECHO: + // echoData(message); default: break; } @@ -37,21 +37,21 @@ void CVBoard::sendMessage() sendColorData(); } -void CVBoard::echoData(const ReceivedSerialMessage& message) -{ - std::string data_string; - memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); +// void CVBoard::echoData(const ReceivedSerialMessage& message) +// { +// std::string data_string; +// memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); - DJISerial::SerialMessage msg; - msg.messageType = CV_MESSAGE_TYPE_ECHO; +// DJISerial::SerialMessage msg; +// msg.messageType = CV_MESSAGE_TYPE_ECHO; - std::string* data = reinterpret_cast(msg.data); +// std::string* data = reinterpret_cast(msg.data); - data = &data_string; +// data = &data_string; - msg.setCRC16(); - drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); -} +// msg.setCRC16(); +// drivers->uart.write(UART_PORT, reinterpret_cast(&msg), sizeof(msg)); +// } void CVBoard::sendOdometryData() { diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp index 6590e56c..c6123f7f 100644 --- a/ut-robomaster/src/communication/cv_board.hpp +++ b/ut-robomaster/src/communication/cv_board.hpp @@ -46,7 +46,7 @@ class CVBoard : public tap::communication::serial::DJISerial /** * Echo the message sent by the CV board to the CV board */ - void echoData(const ReceivedSerialMessage& message); + // void echoData(const ReceivedSerialMessage& message); /** * Sends odometry data to the CV board diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index e4bfa65e..8a85f5df 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -118,6 +118,14 @@ void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS; // rad/s } +void ChassisSubsystem::setOmniVelocities(Vector2f v, float wZ) +{ + targetWheelVels[0] = (v.y + v.x + wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[1] = (v.y - v.x - wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[2] = (v.y - v.x + wZ) / WHEEL_RADIUS; // rad/s + targetWheelVels[3] = (v.y + v.x - wZ) / WHEEL_RADIUS; // rad/s +} + Vector3f ChassisSubsystem::measureVelocity() { float w1 = wheels[0].measureVelocity(); // rev/s diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index dcc70d18..05c0b327 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -36,6 +36,8 @@ class ChassisSubsystem : public Subsystem /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s) Vector3f measureVelocity(); + void setOmniVelocities(Vector2f v, float wZ); + private: src::Drivers* drivers; power_limiter::PowerLimiter powerLimiter; diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index 7feef19e..fa3edcdc 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -7,20 +7,51 @@ void CommandSentryPosition::initialize() { moveTimer.stop(); } void CommandSentryPosition::execute() { - // wait until game starts then wait some more to avoid excess drifting - if (drivers->isGameActive() && moveTimer.isStopped()) + Remote *remote = &drivers->remote; + + Vector2f inputMove = Vector2f( + remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), + remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + + float inputSpin = remote->getChannel(Remote::Channel::WHEEL); + + float inputMoveLen = inputMove.getLength(); + if (inputMoveLen < ANALOG_DEAD_ZONE) + { + inputMove = Vector2f(0.0f); + } + else { - moveTimer.restart(10'000); // 10s + inputMove /= max(1.0f, inputMoveLen); // clamp length } - if (!moveTimer.isExpired()) + if (abs(inputSpin) < ANALOG_DEAD_ZONE) { - chassis->input(Vector2f(0.0f), 0.0f); - return; + inputSpin = 0.0f; } - // speen - chassis->input(Vector2f(0.0f), 1.0f); + // apply quadratic input ramping + inputMove *= inputMove.getLength(); + inputSpin *= abs(inputSpin); + + inputMove *= MAX_LINEAR_VEL; + inputSpin *= MAX_ANGULAR_VEL; + + chassis->setOmniVelocities(inputMove, inputSpin); + + // if (drivers->isGameActive() && moveTimer.isStopped()) + // { + // moveTimer.restart(10'000); // 10s + // } + + // if (!moveTimer.isExpired()) + // { + // chassis->input(Vector2f(0.0f), 0.0f); + // return; + // } + + // // speen + // chassis->input(Vector2f(0.0f), 1.0f); } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } From 04937d0b59bc9071fb08b884db4b237462826ad4 Mon Sep 17 00:00:00 2001 From: suhas Date: Thu, 17 Apr 2025 04:43:26 +0000 Subject: [PATCH 03/19] something something --- ut-robomaster/src/robots/hero/hero_constants.hpp | 7 ++++--- ut-robomaster/src/robots/sentry/sentry_constants.hpp | 4 ++-- ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp | 1 + ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp | 2 ++ .../src/subsystems/chassis/command_sentry_position.cpp | 2 ++ ut-robomaster/src/subsystems/turret/turret_subsystem.cpp | 7 ++++--- 6 files changed, 15 insertions(+), 8 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 0af76a76..219a1df9 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -107,7 +107,7 @@ constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; +const MotorConfig WHEEL_LB{M3508, MOTOR7, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; // flywheels @@ -117,14 +117,15 @@ const MotorConfig FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}}; // agitator -const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; +const MotorConfig + AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; // water wheel const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}}; // turret const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig PITCH{GM6020, MOTOR3, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; const float YAW_OFFSET = 0; const float PITCH_OFFSET = 0; diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp index b4c0d2d2..967a8a5e 100644 --- a/ut-robomaster/src/robots/sentry/sentry_constants.hpp +++ b/ut-robomaster/src/robots/sentry/sentry_constants.hpp @@ -25,8 +25,8 @@ const Sound SOUND_STARTUP = SOUND_SMB_POWERUP; // chassis ------------ static constexpr int WHEELS = 4; -static constexpr float WHEEL_DISTANCE_X = 0.391f; // meters -static constexpr float WHEEL_DISTANCE_Y = 0.315f; // meters +static constexpr float WHEEL_DISTANCE_X = 0.346f; // meters +static constexpr float WHEEL_DISTANCE_Y = 0.346f; // meters static constexpr float WHEEL_RADIUS = 0.1524f; // meters static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f; diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index 8a85f5df..d20bfd93 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -26,6 +26,7 @@ void ChassisSubsystem::initialize() { wheels[i].initialize(); } + logTimer.stop(); } void ChassisSubsystem::refresh() diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp index 05c0b327..f45c5be8 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp @@ -9,6 +9,7 @@ #include "utils/power_limiter/power_limiter.hpp" #include "drivers.hpp" +using tap::arch::MilliTimeout; namespace subsystems::chassis { @@ -43,6 +44,7 @@ class ChassisSubsystem : public Subsystem power_limiter::PowerLimiter powerLimiter; MotorController wheels[WHEELS]; float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f}; + MilliTimeout logTimer; /// @brief Calculate and set wheel velocities for desired robot motion (based on /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf). diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index fa3edcdc..a4530771 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -13,6 +13,8 @@ void CommandSentryPosition::execute() remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + inputMove *= -1; + float inputSpin = remote->getChannel(Remote::Channel::WHEEL); float inputMoveLen = inputMove.getLength(); diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 37f22cf3..ba0074e6 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -29,7 +29,8 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { - setAmputated(!hardwareOk()); + // who needs hardware checks? + // setAmputated(!hardwareOk()); yaw.updateMotorAngle(); pitch.updateMotorAngle(); @@ -37,7 +38,7 @@ void TurretSubsystem::refresh() #if defined(TARGET_STANDARD) || defined(TARGET_HERO) yawEncoder.update(); - if (!isCalibrated && !isAmputated() && yawEncoder.isOnline()) + if (!isCalibrated && yawEncoder.isOnline()) { baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); isCalibrated = true; @@ -54,7 +55,7 @@ void TurretSubsystem::refresh() } #endif - if (isCalibrated && !drivers->isKillSwitched() && !isAmputated()) + if (isCalibrated && !drivers->isKillSwitched()) { yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); From 188fc1b998d67b8df47498291a52c264cce7519c Mon Sep 17 00:00:00 2001 From: arek Date: Fri, 25 Apr 2025 19:06:29 -0500 Subject: [PATCH 04/19] New chasis code --- .../subsystems/chassis/chassis_subsystem.cpp | 4 + .../chassis/command_sentry_position.cpp | 123 +++++++++++++++--- .../chassis/command_sentry_position.hpp | 36 ++++- 3 files changed, 137 insertions(+), 26 deletions(-) diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index d20bfd93..0d71b82d 100644 --- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp +++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp @@ -107,7 +107,11 @@ void ChassisSubsystem::input(Vector2f move, float spin) overdrive -= correction; } +#if defined(TARGET_SENTRY) + setOmniVelocities(v, wZ); +#else setMecanumWheelVelocities(v, wZ); +#endif } void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ) diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index a4530771..812472e7 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -3,43 +3,58 @@ namespace commands { -void CommandSentryPosition::initialize() { moveTimer.stop(); } +// old ard code +// void CommandSentryPosition::initialize() { moveTimer.stop(); } + +void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } void CommandSentryPosition::execute() { - Remote *remote = &drivers->remote; - - Vector2f inputMove = Vector2f( - remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), - remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + float yawAngle = turret->getTargetLocalYaw(); + Vector2f inputMove = Vector2f(0.0f); + float inputSpin = 0.0f; - inputMove *= -1; + // get keyboard input + float keyboardInputSpin; + bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); - float inputSpin = remote->getChannel(Remote::Channel::WHEEL); + // get joystick input + Vector2f joystickInputMove; + float joystickInputSpin; + bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin); - float inputMoveLen = inputMove.getLength(); - if (inputMoveLen < ANALOG_DEAD_ZONE) + // decide which input source to use (keyboard has inertia) + if (!hasKeyboardInput && hasJoystickInput) { - inputMove = Vector2f(0.0f); + inputMove = joystickInputMove; + inputSpin = joystickInputSpin; } else { - inputMove /= max(1.0f, inputMoveLen); // clamp length + inputMove = keyboardInputMove; + inputSpin = keyboardInputSpin; } - if (abs(inputSpin) < ANALOG_DEAD_ZONE) + // auto-align chassis to turret when moving + if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) { - inputSpin = 0.0f; + inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * + CHASSIS_AUTOALIGN_FACTOR; } - // apply quadratic input ramping - inputMove *= inputMove.getLength(); - inputSpin *= abs(inputSpin); + // override spin input while beyblading + if (beyblade) + { + inputSpin = 1.0f; + } - inputMove *= MAX_LINEAR_VEL; - inputSpin *= MAX_ANGULAR_VEL; + // rotate movement vector relative to turret + if (turretRelative) + { + inputMove = inputMove.rotate(yawAngle); + } - chassis->setOmniVelocities(inputMove, inputSpin); + chassis->input(inputMove, inputSpin); // if (drivers->isGameActive() && moveTimer.isStopped()) // { @@ -57,6 +72,74 @@ void CommandSentryPosition::execute() } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } + bool CommandSentryPosition::isFinished() const { return false; } + +bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &inputSpin) +{ + Remote *remote = &drivers->remote; + + inputSpin = 0.0f; // no keyboard spin controls + + Vector2f rawMoveInput = Vector2f( + remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A), + remote->keyPressed(Remote::Key::W) - remote->keyPressed(Remote::Key::S)); + + float rawInputLen = rawMoveInput.getLength(); + + if (rawInputLen > 0.0f) + { + Vector2f moveDir = rawMoveInput / rawInputLen; // normalize input + inputMove += moveDir * KEYBOARD_ACCEL * DT; // incorporate input + inputMove /= max(1.0f, inputMove.getLength()); // clamp length + } + else + { + // decelerate when input stops + float len = inputMove.getLength(); + if (len > 0.0f) + { + inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f); + } + } + + return rawMoveInput != Vector2f(0.0f); +} + + + +bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &inputSpin) +{ + Remote *remote = &drivers->remote; + + inputMove = Vector2f( + remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL), + remote->getChannel(Remote::Channel::RIGHT_VERTICAL)); + + inputSpin = remote->getChannel(Remote::Channel::WHEEL); + + float inputMoveLen = inputMove.getLength(); + if (inputMoveLen < ANALOG_DEAD_ZONE) + { + inputMove = Vector2f(0.0f); + } + else + { + inputMove /= max(1.0f, inputMoveLen); // clamp length + } + + if (abs(inputSpin) < ANALOG_DEAD_ZONE) + { + inputSpin = 0.0f; + } + + // apply quadratic input ramping + inputMove *= inputMove.getLength(); + inputSpin *= abs(inputSpin); + + return inputMove != Vector2f(0.0f) || inputSpin != 0.0f; +} + + } // namespace commands diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp index 69aa2653..0067a949 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp @@ -1,9 +1,16 @@ #pragma once -#include "tap/architecture/timeout.hpp" +//#include "tap/architecture/timeout.hpp" #include "tap/control/command.hpp" -#include "chassis_subsystem.hpp" + +#include "robots/robot_constants.hpp" +//this is older sentry code dont know why it had chopped path +//#include "chassis_subsystem.hpp" +#include "subsystems/chassis/chassis_subsystem.hpp" +#include "subsystems/turret/turret_subsystem.hpp" +#include "utils/chassis_auto_align.hpp" + #include "drivers.hpp" namespace commands @@ -11,14 +18,23 @@ namespace commands using namespace tap::communication::serial; using namespace modm; using subsystems::chassis::ChassisSubsystem; -using tap::arch::MilliTimeout; +// using tap::arch::MilliTimeout; +using subsystems::turret::TurretSubsystem; class CommandSentryPosition : public tap::control::Command { public: - CommandSentryPosition(src::Drivers *drivers, ChassisSubsystem *chassis) + CommandSentryPosition( + src::Drivers *drivers, + ChassisSubsystem *chassis, + TurretSubsystem *turret, + bool turretRelative = false, + bool beyblade = false) : drivers(drivers), - chassis(chassis) + chassis(chassis), + turret(turret), + turretRelative(turretRelative), + beyblade(beyblade) { addSubsystemRequirement(chassis); } @@ -36,6 +52,14 @@ class CommandSentryPosition : public tap::control::Command private: src::Drivers *drivers; ChassisSubsystem *chassis; - MilliTimeout moveTimer; + //MilliTimeout moveTimer; + TurretSubsystem *turret; + + Vector2f keyboardInputMove = Vector2f(0.0f); + const bool turretRelative = false; + const bool beyblade = false; + + bool applyKeyboardInput(Vector2f &moveOut, float &spinOut); + bool applyJoystickInput(Vector2f &moveOut, float &spinOut); }; } // namespace commands \ No newline at end of file From 5108bfd13c42bcd2366fbc748ada5662982baa46 Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 10 Jun 2025 19:31:30 -0500 Subject: [PATCH 05/19] idk --- .../src/robots/hero/hero_constants.hpp | 6 +- .../agitator/agitator_subsystem.cpp | 5 +- .../flywheel/flywheel_subsystem.cpp | 2 +- .../src/subsystems/turret/turret_motor.cpp | 4 +- .../src/subsystems/turret/turret_motor.hpp | 4 +- .../subsystems/turret/turret_subsystem.cpp | 77 +++++++++++-------- 6 files changed, 56 insertions(+), 42 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 219a1df9..05162afd 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -107,7 +107,7 @@ constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR7, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; +const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; // flywheels @@ -125,7 +125,7 @@ const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR3, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; const float YAW_OFFSET = 0; const float PITCH_OFFSET = 0; @@ -141,7 +141,7 @@ static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s const float TARGET_PROJECTILE_VELOCITY = 16; // m/s -const float FLYWHEEL_SPEED = 190.0f; +const float FLYWHEEL_SPEED = 47.5f; const float BALLS_PER_SEC = 4.0f; const float BALLS_PER_REV = 6.0f; diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp index 6bad9af2..91250a5c 100644 --- a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp +++ b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp @@ -46,11 +46,12 @@ void AgitatorSubsystem::refresh() return; #endif - setAmputated(!hardwareOk()); + // setAmputated(!hardwareOk()); float time = getTimeMilliseconds() / 1000.0f; float velocity = getShapedVelocity(time, 1.0f, 0.0f, ballsPerSecond); - bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive(); + // bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive(); + bool killSwitch = drivers->isKillSwitched(); agitator.setActive(!killSwitch); agitator.updateVelocity(velocity); diff --git a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp index b0aa4cc9..5a60d31f 100644 --- a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp +++ b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp @@ -41,7 +41,7 @@ void FlywheelSubsystem::refresh() for (int i = 0; i < FLYWHEELS; i++) { - motors[i].setActive(!killSwitch && !isAmputated()); + motors[i].setActive(!killSwitch); motors[i].updateVelocity(velocity); } } diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index 8273a68a..c5faf15f 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -9,8 +9,8 @@ TurretMotor::TurretMotor( src::Drivers *drivers, MotorConfig motorConfig, const SmoothPidConfig &pidConfig) - : drivers(drivers), - motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name), + : motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name), + drivers(drivers), pid(pidConfig), setpoint(0.0f, 0.0f, M_TWOPI), currentAngle(0.0f, 0.0f, M_TWOPI) diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.hpp b/ut-robomaster/src/subsystems/turret/turret_motor.hpp index 12fcc8d2..d82ee7ef 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.hpp @@ -27,9 +27,11 @@ class TurretMotor float getSetpoint(); bool isOnline(); + DjiMotor motor; + private: src::Drivers *drivers; - DjiMotor motor; + // DjiMotor motor; SmoothPid pid; ContiguousFloat setpoint; float unwrappedAngle = 0; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index ba0074e6..5a31875e 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -1,11 +1,14 @@ #include "turret_subsystem.hpp" +#include "tap/communication/serial/remote.hpp" + #include "modm/math.hpp" #include "robots/robot_constants.hpp" #include "subsystems/subsystem.hpp" namespace subsystems::turret { +using tap::communication::serial::Remote; TurretSubsystem::TurretSubsystem(src::Drivers* drivers) : Subsystem(drivers), drivers(drivers), @@ -29,42 +32,50 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { + // yaw.setOutput(0.5f); + // pitch.motor.setDesiredOutput(M3508.maxOutput * 0.5); // who needs hardware checks? // setAmputated(!hardwareOk()); - yaw.updateMotorAngle(); - pitch.updateMotorAngle(); - -#if defined(TARGET_STANDARD) || defined(TARGET_HERO) - yawEncoder.update(); - - if (!isCalibrated && yawEncoder.isOnline()) - { - baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); - isCalibrated = true; - - setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - } -#else - if (!isCalibrated && !isAmputated()) - { - baseYaw = -YAW_OFFSET; - isCalibrated = true; - - setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - } -#endif - - if (isCalibrated && !drivers->isKillSwitched()) - { - yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); - pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); - } - else - { - yaw.reset(); - pitch.reset(); - } + Remote* remote = &drivers->remote; + float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); + float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); + yaw.setOutput(v); + pitch.motor.setDesiredOutput(GM6020.maxOutput * 0.5f); + + // yaw.updateMotorAngle(); + // pitch.updateMotorAngle(); + + // #if defined(TARGET_STANDARD) || defined(TARGET_HERO) + // yawEncoder.update(); + + // if (!isCalibrated && yawEncoder.isOnline()) + // { + // baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); + // isCalibrated = true; + + // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + // } + // #else + // if (!isCalibrated && !isAmputated()) + // { + // baseYaw = -YAW_OFFSET; + // isCalibrated = true; + + // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + // } + // #endif + + // if (isCalibrated && !drivers->isKillSwitched()) + // { + // yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); + // pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); + // } + // else + // { + // yaw.reset(); + // pitch.reset(); + // } } void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration) From 71b87b7a1a5cf09405824c2e30e913ff091718c3 Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 10 Jun 2025 20:11:35 -0500 Subject: [PATCH 06/19] bad turret: --- ut-robomaster/src/subsystems/turret/turret_subsystem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 5a31875e..3e5e1b18 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -40,8 +40,8 @@ void TurretSubsystem::refresh() Remote* remote = &drivers->remote; float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); - yaw.setOutput(v); - pitch.motor.setDesiredOutput(GM6020.maxOutput * 0.5f); + yaw.setOutput(h); + pitch.motor.setDesiredOutput(GM6020.maxOutput * v); // yaw.updateMotorAngle(); // pitch.updateMotorAngle(); From 0ebe40eb6647d6761dee12c7da3b63e63520db2f Mon Sep 17 00:00:00 2001 From: suhas Date: Wed, 11 Jun 2025 01:40:02 +0000 Subject: [PATCH 07/19] template-project updates --- ut-robomaster/src/main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ut-robomaster/src/main.cpp b/ut-robomaster/src/main.cpp index 21f43fe3..a3e8f9b8 100644 --- a/ut-robomaster/src/main.cpp +++ b/ut-robomaster/src/main.cpp @@ -1,5 +1,6 @@ #include "tap/architecture/periodic_timer.hpp" #include "tap/architecture/profiler.hpp" +#include "tap/motor/motorsim/dji_motor_sim_handler.hpp" #include "robots/robot_constants.hpp" #include "robots/robot_control.hpp" @@ -33,7 +34,7 @@ static void initializeIo(src::Drivers *drivers) static void updateIo(src::Drivers *drivers) { #ifdef PLATFORM_HOSTED - tap::motorsim::SimHandler::updateSims(); + tap::motor::motorsim::DjiMotorSimHandler::getInstance()->updateSims(); #endif drivers->canRxHandler.pollCanData(); From e23e7bc16b08997d474fe857e72bef2c6eb988b0 Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 10 Jun 2025 20:49:13 -0500 Subject: [PATCH 08/19] regenerate taproot --- .../src/tap/algorithms/contiguous_float.cpp | 191 --------------- .../src/tap/algorithms/contiguous_float.hpp | 218 ------------------ ...ear_interpolation_predictor_contiguous.cpp | 55 ----- ...ear_interpolation_predictor_contiguous.hpp | 100 -------- .../src/tap/motor/dji_motor_tx_handler.cpp | 2 - 5 files changed, 566 deletions(-) delete mode 100644 ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp delete mode 100644 ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp delete mode 100644 ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp delete mode 100644 ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp diff --git a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp b/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp deleted file mode 100644 index d49bec0a..00000000 --- a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/*****************************************************************************/ -/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ -/*****************************************************************************/ - -/* - * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington - * - * This file is part of Taproot. - * - * Taproot is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Taproot is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Taproot. If not, see . - */ - -#include "contiguous_float.hpp" - -#include - -namespace tap -{ -namespace algorithms -{ -ContiguousFloat::ContiguousFloat(const float value, const float lowerBound, const float upperBound) -{ - this->value = value; - - this->lowerBound = lowerBound; - this->upperBound = upperBound; - - this->validateBounds(); - this->reboundValue(); -} - -float ContiguousFloat::reboundValue() -{ - if (value < lowerBound) - { - value = upperBound + fmodf(value - lowerBound, upperBound - lowerBound); - } - else if (value > upperBound) - { - value = lowerBound + fmodf(value - upperBound, upperBound - lowerBound); - } - - return value; -} - -float ContiguousFloat::unwrapBelow() const { return lowerBound - (upperBound - value); } - -float ContiguousFloat::unwrapAbove() const { return upperBound + (value - lowerBound); } - -float ContiguousFloat::difference(const float otherValue) const -{ - return difference(ContiguousFloat(otherValue, lowerBound, upperBound)); -} - -float ContiguousFloat::difference(const ContiguousFloat& otherValue) const -{ - // Find the shortest path to the target (smallest difference) - float aboveDiff = otherValue.getValue() - this->unwrapAbove(); - float belowDiff = otherValue.getValue() - this->unwrapBelow(); - float stdDiff = otherValue.getValue() - this->getValue(); - - float finalDiff = stdDiff; - - if (fabs(aboveDiff) < fabs(belowDiff) && fabs(aboveDiff) < fabs(stdDiff)) - { - finalDiff = aboveDiff; - } - else if (fabs(belowDiff) < fabs(aboveDiff) && fabs(belowDiff) < fabs(stdDiff)) - { - finalDiff = belowDiff; - } - - return finalDiff; -} - -void ContiguousFloat::shiftBounds(const float shiftMagnitude) -{ - upperBound += shiftMagnitude; - lowerBound += shiftMagnitude; - reboundValue(); -} - -void ContiguousFloat::shiftValue(const float shiftMagnitude) -{ - value += shiftMagnitude; - reboundValue(); -} - -float ContiguousFloat::limitValue( - const ContiguousFloat& valueToLimit, - const float min, - const float max, - int* status) -{ - ContiguousFloat minContig(min, valueToLimit.lowerBound, valueToLimit.upperBound); - ContiguousFloat maxContig(max, valueToLimit.lowerBound, valueToLimit.upperBound); - return limitValue(valueToLimit, minContig, maxContig, status); -} - -float ContiguousFloat::limitValue( - const ContiguousFloat& valueToLimit, - const ContiguousFloat& min, - const ContiguousFloat& max, - int* status) -{ - if (min.getValue() == max.getValue()) - { - return valueToLimit.getValue(); - } - if ((min.getValue() < max.getValue() && - (valueToLimit.getValue() > max.getValue() || valueToLimit.getValue() < min.getValue())) || - (min.getValue() > max.getValue() && valueToLimit.getValue() > max.getValue() && - valueToLimit.getValue() < min.getValue())) - { - float targetMinDifference = fabs(valueToLimit.difference(min)); - float targetMaxDifference = fabs(valueToLimit.difference(max)); - - if (targetMinDifference < targetMaxDifference) - { - *status = 1; - return min.getValue(); - } - else - { - *status = 2; - return max.getValue(); - } - } - else - { - *status = 0; - return valueToLimit.getValue(); - } -} - -// Getters/Setters ---------------- -// Value -float ContiguousFloat::getValue() const { return value; } - -void ContiguousFloat::setValue(const float newValue) -{ - value = newValue; - this->reboundValue(); -} - -// Upper bound -float ContiguousFloat::getUpperBound() const { return upperBound; } - -void ContiguousFloat::setUpperBound(const float newValue) -{ - upperBound = newValue; - - this->validateBounds(); - this->reboundValue(); -} - -// Lower bound -float ContiguousFloat::getLowerBound() const { return lowerBound; } - -void ContiguousFloat::setLowerBound(const float newValue) -{ - lowerBound = newValue; - - this->validateBounds(); - this->reboundValue(); -} - -void ContiguousFloat::validateBounds() -{ - if (lowerBound > upperBound) - { - float tmp = this->lowerBound; - this->lowerBound = this->upperBound; - this->upperBound = tmp; - } -} - -} // namespace algorithms - -} // namespace tap diff --git a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp b/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp deleted file mode 100644 index 4ee5f516..00000000 --- a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp +++ /dev/null @@ -1,218 +0,0 @@ -/*****************************************************************************/ -/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ -/*****************************************************************************/ - -/* - * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington - * - * This file is part of Taproot. - * - * Taproot is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Taproot is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Taproot. If not, see . - */ - -#ifndef TAPROOT_CONTIGUOUS_FLOAT_HPP_ -#define TAPROOT_CONTIGUOUS_FLOAT_HPP_ - -namespace tap -{ -namespace algorithms -{ -/** - * Wraps a float to allow easy comparison and manipulation of sensor readings - * that wrap (e.g. -180 to 180). - * - * For bounds 0 - 10, logically: - * - 10 + 1 == 1 - * - 0 - 1 == 9 - * - 0 == 10 - * - * Credit to: https://github.com/Team488/SeriouslyCommonLib/blob/af2ce83a830299a8ab3773bec9b8ccc6ab - * 5a3367/src/main/java/xbot/common/math/ContiguousDouble.java - */ -class ContiguousFloat -{ -public: - ContiguousFloat(const float value, const float lowerBound, const float upperBound); - - /** - * Shifts the value so that it still represents the same position but is - * within the current bounds. - * - * @return the new value for chaining functions. - */ - float reboundValue(); - - /** - * Computes the difference between two values (other - this), accounting for - * wrapping. Treats the given 'other' value as a number within the same bounds - * as the current instance. - * - * @param[in] otherValue the other value to compare against. - * @return the computed difference. - */ - float difference(const float otherValue) const; - - /** - * Computes the difference between two values (other - this), accounting for - * wrapping. - * - * @param[in] otherValue the other value to compare against (must have the same bounds - * as the current instance). - * @return the computed difference. - */ - float difference(const ContiguousFloat& otherValue) const; - - /** - * Shifts both bounds by the specified amount - * - * @param[in] shiftMagnitude the amount to add to each bound. - */ - void shiftBounds(const float shiftMagnitude); - - /** - * Shifts value by the specified amount (addition). - * - * @param[in] shiftMagnitude The amount to add to the current value. - */ - void shiftValue(const float shiftMagnitude); - - /** - * Limits the passed in contiguous float between the closest of the - * min or max value if outside the min and max value's wrapped range. - * - * The min and max must have the same wrapped bounds as the valueToLimit. - * - * - * For example given a value wrapped from -10 to 10, with the following - * conditions: - * - valueToLimit: 5, min: 1, max: 4, returns 4. - * - valueToLimit: 9, min: 1, max: 3, returns 1 (since valueToLimit is closest to 1). - * - valueToLimit: 9, min: 2, max: 1, returns 9 (since the range between min and max - * starts at 2, goes up to 9, then wraps around to 1). - * - * @param[in] valueToLimit the ContigousFloat whose value it is to limit - * @param[in] min the ContiguousFloat with the same bounds as valueToLimit that - * valueToLimit will be limited below. - * @param[in] max the ContiguousFloat with the same bounds as valueToLimit that - * valueToLimit will be limited above. - * @param[out] status the status result (what operation the limitValue function performed). The - * status codes are described below: - * - 0: No limiting performed - * - 1: Limited to min value - * - 2: Limited to max value - * @return the limited value. - */ - static float limitValue( - const ContiguousFloat& valueToLimit, - const ContiguousFloat& min, - const ContiguousFloat& max, - int* status); - - /** - * Runs the limitValue function from above, wrapping the min and max passed in to - * the same bounds as those of valueToLimit's. - * - * @see limitValue. - * @param[in] valueToLimit the ContigousFloat whose value it is to limit - * @param[in] min the ContiguousFloat with the same bounds as valueToLimit that - * valueToLimit will be limited below. - * @param[in] max the ContiguousFloat with the same bounds as valueToLimit that - * valueToLimit will be limited above. - * @param[out] status the status result (what operation the limitValue function performed). The - * status codes are described below: - * - 0: No limiting performed - * - 1: Limited to min value - * - 2: Limited to max value - * @return the limited value. - */ - static float limitValue( - const ContiguousFloat& valueToLimit, - const float min, - const float max, - int* status); - - // Getters/Setters ---------------- - - /** - * Returns the wrapped value. - */ - float getValue() const; - - void setValue(const float newValue); - - /** - * Returns the value's upper bound. - */ - float getUpperBound() const; - - /** - * Sets the upper bound to newValue. - */ - void setUpperBound(const float newValue); - - /** - * Returns the value's lower bound. - */ - float getLowerBound() const; - - /** - * Sets the lower bound to newValue. - */ - void setLowerBound(const float newValue); - -private: - /** - * The wrapped value. - */ - float value; - - /** - * The lower bound to wrap around. - */ - float lowerBound; - /** - * The upper bound to wrap around. - */ - float upperBound; - - /** - * Flips the lower and upper bounds if the lower bound is larger than the - * upper bound. - */ - void validateBounds(); - - /** - * Calculates a number representing the current value that is higher than - * (or equal to) the upper bound. Used to make normal numerical comparisons - * without needing to handle wrap cases. - * - * @return the computed value - */ - float unwrapAbove() const; - - /** - * Calculates a number representing the current value that is lower than (or - * equal to) the lower bound. Used to make normal numerical comparisons - * without needing to handle wrap cases. - * - * @return the computed value - */ - float unwrapBelow() const; -}; // class ContiguousFloat - -} // namespace algorithms - -} // namespace tap - -#endif // TAPROOT_CONTIGUOUS_FLOAT_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp deleted file mode 100644 index a0b20378..00000000 --- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/*****************************************************************************/ -/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ -/*****************************************************************************/ - -/* - * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington - * - * This file is part of Taproot. - * - * Taproot is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Taproot is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Taproot. If not, see . - */ - -#include "linear_interpolation_predictor_contiguous.hpp" - -namespace tap::algorithms -{ -LinearInterpolationPredictorContiguous::LinearInterpolationPredictorContiguous( - float lowerBound, - float upperBound) - : lastUpdateCallTime(0), - previousValue(0.0f, lowerBound, upperBound), - slope(0.0f) -{ -} - -void LinearInterpolationPredictorContiguous::update(float newValue, uint32_t currTime) -{ - if (currTime <= lastUpdateCallTime) - { - slope = 0; - return; - } - slope = (previousValue.difference(newValue)) / (currTime - lastUpdateCallTime); - previousValue.setValue(newValue); - lastUpdateCallTime = currTime; -} - -void LinearInterpolationPredictorContiguous::reset(float initialValue, uint32_t initialTime) -{ - previousValue.setValue(initialValue); - lastUpdateCallTime = initialTime; - slope = 0.0f; -} -} // namespace tap::algorithms diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp deleted file mode 100644 index 5d69b46f..00000000 --- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp +++ /dev/null @@ -1,100 +0,0 @@ -/*****************************************************************************/ -/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ -/*****************************************************************************/ - -/* - * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington - * - * This file is part of Taproot. - * - * Taproot is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Taproot is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Taproot. If not, see . - */ - -#ifndef TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_ -#define TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_ - -#include - -#include "contiguous_float.hpp" - -namespace tap::algorithms -{ -/** - * An object that is similar in every respect to the `LinearInterpolationPredictor` - * object except that it uses `ContiguousFloat`'s instead. - */ -class LinearInterpolationPredictorContiguous -{ -public: - /** - * @param[in] lowerBound Lower bound for linear interpolation contiguous float. - * @param[in] upperBound Upper bound for linear interpolation contiguous float. - */ - LinearInterpolationPredictorContiguous(float lowerBound, float upperBound); - - /** - * Updates the interpolation using the newValue. - * - * @note Only call this when you receive a new value (use remote rx - * counter to tell when there is new data from the remote, for - * example). - * @note This function should be called with increasing values of `currTime`. - * @param[in] newValue The new data used in the interpolation. - * @param[in] currTime The time that this function was called. - */ - void update(float newValue, uint32_t currTime); - - /** - * Returns the current value, that is: \f$y\f$ in the equation - * \f$y=slope\cdot (currTime - lastUpdateCallTime) + previousValue\f$ - * in the units of whatever value you are inputting in the `update` function. - * - * @note Slope is defined by the previous two values passed into the `update` - * function, a period preceeding `lastUpdateCallTime`. - * @note Use a millisecond-resolution timer, e.g. - * `tap::arch::clock::getTimeMilliseconds()`. - * @param[in] currTime The current clock time, in ms. - * @return The interpolated value. - */ - float getInterpolatedValue(uint32_t currTime) - { - return ContiguousFloat( - slope * static_cast(currTime - lastUpdateCallTime) + - previousValue.getValue(), - previousValue.getLowerBound(), - previousValue.getUpperBound()) - .getValue(); - } - - /** - * Resets the predictor. The slope will be reset to 0 and the initial values - * and time will be used to initialize the predictor. - * - * @note It is highly recommended that you call this function before calling - * `update` to "initialize" the system. - * - * @param[in] initialValue The value to set the previous value to when resetting. - * @param[in] initialTime The value to set the previous time to when resetting. - */ - void reset(float initialValue, uint32_t initialTime); - -private: - uint32_t lastUpdateCallTime; ///< The previous timestamp from when update was called. - ContiguousFloat previousValue; ///< The previous data value. - float slope; ///< The current slope, calculated using the previous and most current data. -}; // class LinearInterpolationPredictorContiguous - -} // namespace tap::algorithms - -#endif // TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_ diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp index 9de73908..6c8cc2fb 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp @@ -113,8 +113,6 @@ void DjiMotorTxHandler::encodeAndSendCanData() if (can1ValidMotorMessageHigh) { messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1MessageHigh); - can1MessageHigh.setIdentifier(0x1FE); - messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1MessageHigh); } } if (drivers->can.isReadyToSend(can::CanBus::CAN_BUS2)) From a1d86b42737988bcef028487c7cb4343fc214485 Mon Sep 17 00:00:00 2001 From: suhas Date: Wed, 11 Jun 2025 01:59:29 +0000 Subject: [PATCH 09/19] taproot 2.0 up --- .../src/subsystems/turret/double_yaw_motor.cpp | 12 ++++++------ .../src/subsystems/turret/double_yaw_motor.hpp | 6 +++--- ut-robomaster/src/subsystems/turret/turret_motor.cpp | 12 ++++++------ ut-robomaster/src/subsystems/turret/turret_motor.hpp | 6 +++--- .../src/subsystems/turret/turret_subsystem.hpp | 4 ++-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp index 9b06d677..b4689c2e 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp @@ -36,15 +36,15 @@ void DoubleYawMotor::updateMotorAngle() { float encoderAngle = static_cast(motor1.getEncoderUnwrapped()) / DjiMotor::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; - currentAngle.setValue(encoderAngle); + currentAngle.setWrappedValue(encoderAngle); } void DoubleYawMotor::setAngle(float desiredAngle, float dt) { - setpoint.setValue(desiredAngle / M_TWOPI); + setpoint.setWrappedValue(desiredAngle / M_TWOPI); - float positionError = - ContiguousFloat(currentAngle.getValue(), 0, 1.0f).difference(setpoint.getValue()); + float positionError = WrappedFloat(currentAngle.getWrappedValue(), 0, 1.0f) + .minDifference(setpoint.getWrappedValue()); // account for chassis rotation float disturbance = drivers->bmi088.getGz() / 360.0f; // rev / s @@ -54,7 +54,7 @@ void DoubleYawMotor::setAngle(float desiredAngle, float dt) setVelocity(targetVelocity, dt); } -float DoubleYawMotor::getAngle() { return currentAngle.getValue() * M_TWOPI; } +float DoubleYawMotor::getAngle() { return currentAngle.getWrappedValue() * M_TWOPI; } void DoubleYawMotor::setVelocity(float velocity, float dt) { @@ -78,6 +78,6 @@ void DoubleYawMotor::setOutput(float output) float DoubleYawMotor::getOutput() { return motor1.getOutputDesired() / M3508.maxOutput; } -float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); } +float DoubleYawMotor::getSetpoint() { return setpoint.getWrappedValue(); } bool DoubleYawMotor::isOnline() { return motor1.isMotorOnline() && motor2.isMotorOnline(); } } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp index 4e270ecf..96655bec 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp @@ -1,7 +1,7 @@ #pragma once -#include "tap/algorithms/contiguous_float.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/algorithms/wrapped_float.hpp" #include "tap/motor/motor_interface.hpp" #include "utils/motors/motor_constants.hpp" @@ -38,7 +38,7 @@ class DoubleYawMotor DjiMotor motor2; motor_controller::Pid velocityPid; motor_controller::Pid positionPid; - ContiguousFloat setpoint; - ContiguousFloat currentAngle; + WrappedFloat setpoint; + WrappedFloat currentAngle; }; } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index c5faf15f..6d5c74a8 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -34,25 +34,25 @@ void TurretMotor::updateMotorAngle() unwrappedAngle = static_cast(encoderValue) * M_TWOPI / static_cast(DjiMotor::ENC_RESOLUTION); - currentAngle.setValue(unwrappedAngle); + currentAngle.setWrappedValue(unwrappedAngle); } } void TurretMotor::setAngle(float desiredAngle, float dt) { - setpoint.setValue(desiredAngle); + setpoint.setWrappedValue(desiredAngle); - float positionControllerError = - ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue()); + float positionControllerError = WrappedFloat(currentAngle.getWrappedValue(), 0, M_TWOPI) + .minDifference(setpoint.getWrappedValue()); float output = pid.runController(positionControllerError, (M_TWOPI / 60.0f) * motor.getShaftRPM(), dt); motor.setDesiredOutput(output); } -float TurretMotor::getAngle() { return currentAngle.getValue(); } +float TurretMotor::getAngle() { return currentAngle.getWrappedValue(); } -float TurretMotor::getSetpoint() { return setpoint.getValue(); } +float TurretMotor::getSetpoint() { return setpoint.getWrappedValue(); } bool TurretMotor::isOnline() { return motor.isMotorOnline(); } } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.hpp b/ut-robomaster/src/subsystems/turret/turret_motor.hpp index d82ee7ef..7cbf65a0 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.hpp @@ -1,7 +1,7 @@ #pragma once -#include "tap/algorithms/contiguous_float.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/algorithms/wrapped_float.hpp" #include "tap/motor/motor_interface.hpp" #include "utils/motors/motor_constants.hpp" @@ -33,9 +33,9 @@ class TurretMotor src::Drivers *drivers; // DjiMotor motor; SmoothPid pid; - ContiguousFloat setpoint; + WrappedFloat setpoint; float unwrappedAngle = 0; - ContiguousFloat currentAngle; + WrappedFloat currentAngle; float lastUpdatedEncoderValue = 0; }; } // namespace subsystems::turret \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index 630e02eb..90a92433 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp @@ -1,6 +1,6 @@ #pragma once -#include "tap/algorithms/contiguous_float.hpp" +#include "tap/algorithms/wrapped_float.hpp" #include "tap/control/subsystem.hpp" #include "drivers/as5600.hpp" @@ -16,7 +16,7 @@ namespace subsystems::turret { using driver::As5600; using modm::Vector3f; -using tap::algorithms::ContiguousFloat; +using tap::algorithms::WrappedFloat; class TurretSubsystem : public Subsystem { From 5daaf267e6819d1bab6fa4b3de5ee16b75578f60 Mon Sep 17 00:00:00 2001 From: suhas Date: Wed, 11 Jun 2025 19:23:29 -0500 Subject: [PATCH 10/19] regenerate taproot --- taproot | 2 +- .../taproot/src/tap/algorithms/MahonyAHRS.cpp | 15 +- .../taproot/src/tap/algorithms/MahonyAHRS.h | 40 +- .../src/tap/algorithms/butterworth.hpp | 467 ++++++++++++++++ .../src/tap/algorithms/discrete_filter.hpp | 170 ++++++ .../odometry/odometry_2d_interface.hpp | 7 + .../odometry/odometry_2d_tracker.cpp | 6 + .../odometry/odometry_2d_tracker.hpp | 2 + .../transforms/angular_velocity.hpp | 83 +++ .../transforms/dynamic_orientation.hpp | 133 +++++ .../transforms/dynamic_position.hpp | 131 +++++ .../tap/algorithms/transforms/orientation.hpp | 24 +- .../tap/algorithms/transforms/position.hpp | 3 + .../tap/algorithms/transforms/transform.cpp | 282 +++++++++- .../tap/algorithms/transforms/transform.hpp | 512 ++++++++++++++++-- .../src/tap/algorithms/transforms/vector.cpp | 6 - .../src/tap/algorithms/transforms/vector.hpp | 20 +- .../src/tap/algorithms/wrapped_float.cpp | 12 +- .../src/tap/algorithms/wrapped_float.hpp | 2 +- .../tap/communication/can/can_rx_handler.cpp | 62 ++- .../tap/communication/can/can_rx_handler.hpp | 39 +- .../tap/communication/can/can_rx_listener.hpp | 11 + .../sensors/encoder/encoder_interface.hpp | 65 +++ .../sensors/encoder/multi_encoder.hpp | 203 +++++++ .../sensors/encoder/wrapped_encoder.cpp | 128 +++++ .../sensors/encoder/wrapped_encoder.hpp | 119 ++++ .../sensors/imu/abstract_imu.cpp | 101 ++++ .../sensors/imu/abstract_imu.hpp | 144 +++++ .../sensors/imu/bmi088/bmi088.cpp | 117 +--- .../sensors/imu/bmi088/bmi088.hpp | 106 +--- .../sensors/imu/imu_interface.hpp | 36 +- .../imu/imu_terminal_serial_handler.hpp | 6 +- .../voltage/voltage_sensor_interface.hpp | 44 ++ .../tap/communication/serial/ref_serial.cpp | 27 +- .../tap/communication/serial/ref_serial.hpp | 21 +- .../communication/serial/ref_serial_data.hpp | 108 ++-- .../serial/ref_serial_transmitter.cpp | 7 +- .../tcp-server/json_messages.cpp | 8 +- .../tcp-server/json_messages.hpp | 6 +- .../communication/tcp-server/tcp_server.cpp | 2 + .../chassis/chassis_subsystem_interface.hpp | 1 - .../src/tap/control/chassis/power_limiter.cpp | 7 +- .../src/tap/control/chassis/power_limiter.hpp | 3 + .../setpoint/commands/calibrate_command.hpp | 1 - .../setpoint/commands/move_command.hpp | 1 - .../setpoint/commands/unjam_command.hpp | 2 - .../taproot/src/tap/motor/dji_motor.cpp | 71 +-- .../taproot/src/tap/motor/dji_motor.hpp | 136 ++--- .../src/tap/motor/dji_motor_encoder.cpp | 69 +++ .../src/tap/motor/dji_motor_encoder.hpp | 112 ++++ .../taproot/src/tap/motor/dji_motor_ids.hpp | 50 ++ .../dji_motor_terminal_serial_handler.cpp | 4 +- .../dji_motor_terminal_serial_handler.hpp | 3 +- .../src/tap/motor/dji_motor_tx_handler.cpp | 41 +- .../src/tap/motor/dji_motor_tx_handler.hpp | 12 +- .../src/tap/motor/double_dji_motor.cpp | 55 +- .../src/tap/motor/double_dji_motor.hpp | 17 +- .../taproot/src/tap/motor/motor_interface.hpp | 23 +- .../src/tap/motor/motorsim/can_serializer.hpp | 2 +- .../test/tap/mock/abstract_imu_mock.cpp | 30 + .../test/tap/mock/abstract_imu_mock.hpp | 54 ++ .../taproot/test/tap/mock/bmi088_mock.hpp | 25 +- .../test/tap/mock/dji_motor_encoder_mock.cpp | 50 ++ .../test/tap/mock/dji_motor_encoder_mock.hpp | 60 ++ .../taproot/test/tap/mock/dji_motor_mock.cpp | 12 +- .../taproot/test/tap/mock/dji_motor_mock.hpp | 14 +- .../tap/mock/dji_motor_tx_handler_mock.cpp | 2 + .../test/tap/mock/encoder_interface_mock.cpp | 30 + .../test/tap/mock/encoder_interface_mock.hpp | 54 ++ .../test/tap/mock/imu_interface_mock.hpp | 19 +- .../mock/imu_terminal_serial_handler_mock.cpp | 2 +- .../mock/imu_terminal_serial_handler_mock.hpp | 2 +- .../test/tap/mock/motor_interface_mock.cpp | 2 +- .../test/tap/mock/motor_interface_mock.hpp | 16 +- .../tap/mock/odometry_2d_interface_mock.hpp | 6 + .../taproot/test/tap/mock/ref_serial_mock.hpp | 2 +- 76 files changed, 3543 insertions(+), 724 deletions(-) create mode 100644 ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp create mode 100644 ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp create mode 100644 ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp create mode 100644 ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp create mode 100644 ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp create mode 100644 ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp create mode 100644 ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp create mode 100644 ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp create mode 100644 ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp create mode 100644 ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp create mode 100644 ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp create mode 100644 ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp create mode 100644 ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp create mode 100644 ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp create mode 100644 ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp diff --git a/taproot b/taproot index 22b708e6..2a6bf7c7 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f +Subproject commit 2a6bf7c7e30827d6aa210bee8125d2e7bc28862a diff --git a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp index 985f1b66..6cea9545 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp @@ -58,7 +58,6 @@ Mahony::Mahony() integralFBx = 0.0f; integralFBy = 0.0f; integralFBz = 0.0f; - anglesComputed = 0; invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ; roll = 0.0f; pitch = 0.0f; @@ -87,11 +86,6 @@ void Mahony::update( return; } - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) @@ -189,7 +183,6 @@ void Mahony::update( q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; - anglesComputed = 0; } //------------------------------------------------------------------------------------------- @@ -200,11 +193,6 @@ void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float a float recipNorm; float qa, qb, qc; - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) @@ -271,7 +259,7 @@ void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float a q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; - anglesComputed = 0; + computeAngles(); } //------------------------------------------------------------------------------------------- @@ -281,7 +269,6 @@ void Mahony::computeAngles() roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); - anglesComputed = 1; } template diff --git a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h index 20c741ec..463c6b3a 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h +++ b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h @@ -13,6 +13,8 @@ // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // 09/06/2020 Matthew Arnold Update style, use safer casting +// 04/30/2022 Matthew Arnold Change input/outputs from degrees to radians +// 03/06/2025 Chinmay Murthy Make getters const // //============================================================================================= #ifndef MAHONY_AHRS_H_ @@ -20,6 +22,8 @@ #include +#include "modm/math/geometry/angle.hpp" + //-------------------------------------------------------------------------------------------- // Variable declaration @@ -32,7 +36,6 @@ class Mahony float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki float invSampleFreq; float roll, pitch, yaw; - char anglesComputed; static float invSqrt(float x); void computeAngles(); @@ -56,7 +59,6 @@ class Mahony integralFBx = 0.0f; integralFBy = 0.0f; integralFBz = 0.0f; - anglesComputed = 0; roll = 0.0f; pitch = 0.0f; yaw = 0.0f; @@ -72,37 +74,9 @@ class Mahony float my, float mz); void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); - float getRoll() - { - if (!anglesComputed) computeAngles(); - return roll * 57.29578f; - } - float getPitch() - { - if (!anglesComputed) computeAngles(); - return pitch * 57.29578f; - } - float getYaw() - { - if (!anglesComputed) computeAngles(); - float yawDegrees = yaw * 57.29578f; - return fmod(yawDegrees + 360.0f, 360.0f); - } - float getRollRadians() - { - if (!anglesComputed) computeAngles(); - return roll; - } - float getPitchRadians() - { - if (!anglesComputed) computeAngles(); - return pitch; - } - float getYawRadians() - { - if (!anglesComputed) computeAngles(); - return yaw; - } + float getRoll() const { return roll; } + float getPitch() const { return pitch; } + float getYaw() const { return fmod(yaw + M_TWOPI, M_TWOPI); } }; #endif // MAHONY_AHRS_H_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp new file mode 100644 index 00000000..d2f53634 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp @@ -0,0 +1,467 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_BUTTERWORTH_HPP_ +#define TAPROOT_BUTTERWORTH_HPP_ +#include +#include +#include +#include + +#include "modm/math/geometry/angle.hpp" + +#include "discrete_filter.hpp" + +/** + * @brief Implementation of Butterworth filter design in the discrete domain. + * + * This header file provides a implementation of Butterworth filters, + * including low-pass, high-pass, band-pass, and band-stop filters. The Butterworth + * filter is known for its maximally flat frequency response in the passband, making + * it ideal for applications requiring minimal signal distortion. + * + * The implementation includes: + * - Conversion of poles and zeros from the Laplace domain to the Z domain using + * the bilinear transform. + * - Expansion of polynomial coefficients from a set of poles or zeros. + * - Evaluation of the frequency response of the filter at a given frequency. + * - A templated Butterworth filter class for designing filters of arbitrary order + * and type. + * + * The design process includes pre-warping of frequencies for the bilinear transform, + * generation of prototype poles, and scaling of coefficients. + * + * The following transforms map a lowpass prototype into other filter types (s-domain): + * + * - **Lowpass to Lowpass**: + * \f$ s \rightarrow \frac{s}{\Omega_c} \f$ + * + * - **Lowpass to Highpass**: + * \f$ s \rightarrow \frac{\Omega_c}{s} \f$ + * + * - **Lowpass to Bandpass**: + * \f$ s \rightarrow \frac{s^2 + \Omega_0^2}{B s} \f$ + * + * - **Lowpass to Bandstop**: + * \f$ s \rightarrow \frac{B s}{s^2 + \Omega_0^2} \f$ + * + * Where: + * \f$ \Omega_0 = \sqrt{\Omega_l \cdot \Omega_h}, \quad B = \Omega_h - \Omega_l \f$ + * + * After analog transformation, apply the bilinear transform: + * \f[ s = \frac{2}{T} \cdot \frac{z - 1}{z + 1} \f] + * + * @note + * This implementation is designed for C++20 ``constexpr``. + * + * @warning + * High-order filters can introduce high phase delays and should be used with caution. + * For most applications, low-pass and high-pass filters of order 2 or lower are sufficient. + * + * If results are suspicious, verify filter coefficients using external tools + * such as MATLAB or Python (e.g., SciPy). + * + * + * @section Usage + * + * To use this implementation, include this header file and instantiate the + * ``Butterworth`` class with the desired filter order, type, and parameters. + * Then, pass those coefficients into a ``DiscreteFilter``. + * + * @code + * Coefficients coe = butterworth<1, LOWPASS>(wc, Ts); + * DiscreteFilter<2> Filter(coe); + * + * @endcode + * + * @author Aiden Prevey + * @date 4/29/2025 + * @version 2.0 + */ + +namespace tap::algorithms::filter +{ +enum FilterType : uint8_t +{ + LOWPASS = 0b00, + HIGHPASS = 0b01, + BANDPASS = 0b10, + BANDSTOP = 0b11, +}; + +/** + * used to transform poles from the laplace domain to the + * Z domain for discrete time using the bilinear transform + * + * @param [in] s a pole or zero from the laplace domain + * @param [in] Ts the sample time + * @return a complex number corresponding to the Z domain location + */ +constexpr std::complex s2z(std::complex s, double Ts) +{ + return (static_cast>(1.0) + (Ts / 2) * s) / + (static_cast>(1.0) - (Ts / 2) * s); +} + +/** + * used to multiply out a series of zeros to obtain a list of coefficients + * + * @param [in] zeros a vector of complex poles or zeros to multiply out, works + * for any polynomial in the form of (x - z1)(x - z2)...(x - zn) where z1, z2, ... zn + * are the value of zeros. + * + * @return a vector of coefficients for the polynomial with the 0th index being the + * constant term and the last index being the leading coefficient + * @note the coefficients are returned as doubles, the imaginary part of the + * complex number is ignored + */ +template +constexpr std::array expandPolynomial( + std::array, ORDER> zeros) +{ + std::array, ORDER + 1> coefficients{ + std::complex(1.0, 0.0)}; // Start with 1 + + for (size_t i = 0; i < ORDER; i++) + { + // Multiply current polynomial by (x - zero) + std::array, ORDER + 1> newCoefficients{std::complex(0.0, 0.0)}; + for (size_t j = 0; j < i + 1; j++) + { + newCoefficients[j] -= + coefficients[j] * zeros[i]; // Multiply current coefficient by zero + newCoefficients[j + 1] += coefficients[j]; // Shift current coefficient + } + coefficients.swap(newCoefficients); + } + std::array stripped_coefficients{}; + // Extract the real part of each coefficient, the imaginary appears to be an + // artifact of double + for (size_t i = 0; i < ORDER + 1; i++) + { + stripped_coefficients[i] = coefficients[i].real(); + } + + return stripped_coefficients; +} + +/** + * used to evaluate the frequency response of a filter at a given frequency + * @param [in] b the numerator coefficients of the filter + * @param [in] a the denominator coefficients of the filter + * @param [in] w the frequency to evaluate at in rad/s + * @param [in] Ts the sample time + * @return the frequency response of the filter at the given frequency + * + */ + +template +constexpr std::complex evaluateFrequencyResponse( + const std::array &b, + const std::array &a, + double w, + double Ts) +{ + std::complex j(0, 1); // imaginary unit i = sqrt(-1) + std::complex numerator(0, 0); + std::complex denominator(0, 0); + + double omega = w * Ts; // omega is in terms of rad/sample + + // Evaluate numerator: b0 + b1 * e^{-jω} + b2 * e^{-j2ω} + ... + for (size_t k = 0; k < b.size(); ++k) + { + numerator += b[k] * (std::cos(omega * static_cast(k)) - + j * std::sin(omega * static_cast(k))); + } + + // Evaluate denominator: a0 + a1 * e^{-jω} + a2 * e^{-j2ω} + ... + for (size_t k = 0; k < a.size(); ++k) + { + denominator += a[k] * (std::cos(omega * static_cast(k)) - + j * std::sin(omega * static_cast(k))); + } + + return numerator / denominator; +} +/** + * used to calculate the magnitude of a complex number, created for constexpr useage + * @param [in] z the complex number to calculate the magnitude of + * @return the magnitude of the complex number + */ +constexpr double complexAbs(std::complex z) +{ + return std::sqrt(z.real() * z.real() + z.imag() * z.imag()); +} + +/** + * used to calculate the square root of a complex number, created for constexpr useage + * @param [in] z the complex number to calculate the square root of + * @return the square root of the complex number + */ + +constexpr std::complex complexSqrt(std::complex z) +{ + double r = std::sqrt(complexAbs(z)); + double theta = static_cast(std::atan2(z.imag(), z.real())) * static_cast(0.5f); + return {r * std::cos(theta), r * std::sin(theta)}; +} + +constexpr uint16_t getNumCoefficients(uint8_t ORDER, FilterType type) +{ + return (1 + ((type & 0b10) != 0)) * ORDER + 1; +} + +/** + * @param[in] wc for LOW/HIGHPASS: cutoff ωc. + * for BANDPASS/BANDSTOP: lower edge ωl. + * @param[in] Ts sample time. + * @param[in] type filter type, LOWPASS, HIGHPASS, BANDPASS, BANDSTOP. + * defaults to LOWPASS. + * @param[in] wh upper edge ωh (only used for band filters). + */ +template +Coefficients constexpr butterworth( + double wc, + double Ts, + double wh = 0.0) +{ + const uint16_t COEFFICIENTS = getNumCoefficients(ORDER, Type); + + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; + + const int n = ORDER; + + // For band filters we treat wc as ωl + double wl = wc; + double whp = wh; + std::array, 2 * ORDER> bandpass_stop_poles; + + // pre-warp all edges for bilinear transform + wl = static_cast(2.0) / Ts * std::tan(wl * (Ts / static_cast(2.0))); + whp = static_cast(2.0) / Ts * std::tan(whp * (Ts / static_cast(2.0))); + + // generate N prototype poles on unit circle + std::array, COEFFICIENTS - 1> poles; + for (int k = 0; k < n; ++k) + { + double theta = M_PI * (2.0 * k + 1) / (2.0 * n) + M_PI / 2.0; + poles[k] = std::complex(std::cos(theta), std::sin(theta)); + } + + std::array, COEFFICIENTS - 1> zPoles; + + // apply the appropriate s-domaisn transform to each pole + switch (Type) + { + case LOWPASS: + { + // poles are multiplied by wl to scale them to the desired cutoff frequency + for (int j = 0; j < n; ++j) poles[j] = poles[j] * wl; + + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); + break; + } + case HIGHPASS: + { + // applys the inverse transform of the butterworth lowpass filter + for (int j = 0; j < n; ++j) + { + poles[j] = wl / poles[j]; + } + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); + break; + } + // In the case of a bandpass or a bandstop the amount of poles doubles. + case BANDPASS: + { + /* + * transform in the form of s → (s² + Ω₀²) / (B · s) + * where: + * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) + * B = Ω_high - Ω_low // Bandwidth (rad/sec) + */ + double B = whp - wl; + double W0sq = whp * wl; + + for (int j = 0; j < ORDER; ++j) + { + std::complex p = poles[j]; + + std::complex discriminant = + (p * B) * (p * B) - static_cast>(4.0) * W0sq; + std::complex root = complexSqrt(discriminant); + + bandpass_stop_poles[2 * j] = + (p * B + root) * static_cast>(0.5); + bandpass_stop_poles[2 * j + 1] = + (p * B - root) * static_cast>(0.5); + } + + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(bandpass_stop_poles[i], Ts); + + break; + } + + case BANDSTOP: + { + /* + * transform in the form of s → B · s / (s² + Ω₀²) + * where: + * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) + * B = Ω_high - Ω_low // Bandwidth (rad/sec) + */ + double B = whp - wl; + double W0sq = whp * wl; + for (int j = 0; j < n; ++j) + { + std::complex p = poles[j]; + + std::complex discriminant = + B * B - (static_cast>(4.0) * -p * W0sq); + std::complex root = complexSqrt(discriminant); + + bandpass_stop_poles[2 * j] = + (B + root) / (static_cast>(2.0) * p); + bandpass_stop_poles[2 * j + 1] = + (B - root) / (static_cast>(2.0) * p); + } + + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(bandpass_stop_poles[i], Ts); + + break; + } + } + + std::array, COEFFICIENTS - 1> zZeros; + + switch (Type) + { + case LOWPASS: + // zeros: for Butterworth lowpass all z-zeros at z = –1 + zZeros.fill(std::complex(-1, 0)); + break; + case HIGHPASS: + // zeros: for butterworth highpass all z-zeros are at z = 1 + zZeros.fill(std::complex(1, 0)); + break; + case BANDPASS: + // zeros: for butterworth bandpass all z-zeros are at z = ±1 + for (int i = 0; i < COEFFICIENTS - 1; ++i) + { + zZeros[i] = (i % 2 == 0) ? std::complex(1, 0) : std::complex(-1, 0); + } + break; + case BANDSTOP: + { + /* zeros: for butterworth bandstop are in a circle with radius 1 at the + * center of the z-domain with an angle in radians per sample of the + * center frequency. The zeros are complex conjugates of each other. + */ + + /* the notch (center) frequency in radians/sample */ + double omega0 = std::sqrt(wl * whp) * Ts; + + double realPart = std::cos(omega0); + double imagPart = std::sin(omega0); + + std::complex zeroPlus(realPart, imagPart); // e^(+jω0) + std::complex zeroMinus(realPart, -imagPart); // e^(-jω0) + + for (int i = 0; i < COEFFICIENTS - 1; i += 2) + { + zZeros[i] = zeroPlus; // first of pair + if (i + 1 < COEFFICIENTS) zZeros[i + 1] = zeroMinus; // second of pair + } + } + break; + } + + // get expanded polynomials + auto b = expandPolynomial(zZeros); + auto a = expandPolynomial(zPoles); + + // scale the gain properly + switch (Type) + { + case LOWPASS: + { + // Eval at DC + auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); + auto mag = complexAbs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + case HIGHPASS: + { + // Eval at nyquist + auto freqResp = evaluateFrequencyResponse( + b, + a, + static_cast(M_PI) / Ts, + Ts); + auto mag = complexAbs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + case BANDPASS: + { + // Eval at center frequency + auto freqResp = + evaluateFrequencyResponse(b, a, std::sqrt(wl * whp), Ts); + auto mag = complexAbs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + case BANDSTOP: + { + // Eval at dc gain + auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); + auto mag = complexAbs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + } + + // store in member arrays (reverse order) + for (size_t i = 0; i < COEFFICIENTS; ++i) + { + naturalResponseCoefficients[COEFFICIENTS - i - 1] = a[i]; + forcedResponseCoefficients[COEFFICIENTS - i - 1] = b[i]; + } + + return {naturalResponseCoefficients, forcedResponseCoefficients}; +} + +} // namespace tap::algorithms::filter + +#endif // TAPROOT_BUTTERWORTH_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp new file mode 100644 index 00000000..8e69f207 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp @@ -0,0 +1,170 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_DISCRETE_FILTER_HPP_ +#define TAPROOT_DISCRETE_FILTER_HPP_ + +#include +#include + +namespace tap::algorithms::filter +{ +/** + * @struct Coefficients + * @brief Represents the coefficients used in a discrete filter. + * + * This structure holds two sets of coefficients: + * - `naturalResponseCoefficients`: Coefficients related to the natural response of the system. + * - `forcedResponseCoefficients`: Coefficients related to the forced response of the system. + * + * @tparam T The data type of the coefficients (e.g., float, double). + * @tparam SIZE The size of the coefficient arrays. + */ +template +struct Coefficients +{ + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; +}; + +/** + * @brief DiscreteFilter class implements a discrete-time filter using the finite difference + * equation. + * @tparam SIZE The size of the filter coefficients. + * + * This class provides methods to filter input data using the finite difference equation and + * maintain the internal state of the filter. + */ +template +class DiscreteFilter +{ +public: + /** + * @brief Constructor for the DiscreteFilter class. + * @param [in] naturalResponseCoefficients The coefficients for the natural response (a). + * @param [in] forcedResponseCoefficients The coefficients for the forced response (b). + * + * This constructor initializes the filter with the given coefficients and resets the filter + * state to zero. + */ + DiscreteFilter( + std::array naturalResponseCoefficients, + std::array forcedResponseCoefficients) + : naturalResponseCoefficients(naturalResponseCoefficients), + forcedResponseCoefficients(forcedResponseCoefficients) + { + reset(); + } + + DiscreteFilter(const Coefficients coefficients) + : naturalResponseCoefficients(coefficients.naturalResponseCoefficients), + forcedResponseCoefficients(coefficients.forcedResponseCoefficients) + { + reset(); + } + + /** + * @brief Filters the input data using the finite difference equation. + * @param [in] dat The input data to be filtered. + * @return The filtered output data. + * + * This function implements a discrete-time filter using the finite difference equation. + * It updates the internal state of the filter based on the input data and returns the + * filtered output. + * + * \f$ y(n)=\frac{1}{a_{0}}\left[\sum_{\kappa=0}^{M} b_{\kappa}x(n-\kappa)-\sum_{k=1}^{N} + * a_{k}y(n-k)\right]. \qquad{(2)} \f$ + * + */ + T filterData(float dat) + { + for (int i = SIZE - 1; i > 0; i--) + { + forcedResponse[i] = forcedResponse[i - 1]; + } + forcedResponse[0] = dat; + + float sum = 0; + // Sum of forced response coefficients multiplied by the forced response X(n-k) + // (previous input data) + for (int i = 0; i < SIZE; i++) + { + sum += forcedResponseCoefficients[i] * forcedResponse[i]; + } + // Sum of natural response coefficients multiplied by the natural response Y(n-k) + // (previous output data) + for (int i = 0; i < SIZE - 1; i++) + { + sum -= naturalResponseCoefficients[i + 1] * naturalResponse[i]; + } + // Apply the 1/a_0 scaling to the output + sum /= naturalResponseCoefficients[0]; + + // Shift the natural response array to make room for the new output + for (int i = SIZE - 1; i > 0; i--) + { + naturalResponse[i] = naturalResponse[i - 1]; + } + + naturalResponse[0] = sum; + + return naturalResponse[0]; + } + + /** @brief Returns the last filtered value*/ + T getLastFiltered() { return naturalResponse[0]; } + + /** @brief Resets the filter's state to zero, keeps the coefficients */ + + T reset() + { + // Reset the filter state to zero + naturalResponse.fill(0.0f); + forcedResponse.fill(0.0f); + return 0.0f; + } + + void setCoefficients(Coefficients coe) + { + this->naturalResponseCoefficients = coe.naturalResponseCoefficients; + this->forcedResponseCoefficients = coe.forcedResponseCoefficients; + } + + void setCoefficients( + std::array naturalResponseCoefficients, + std::array forcedResponseCoefficients) + { + this->naturalResponseCoefficients = naturalResponseCoefficients; + this->forcedResponseCoefficients = forcedResponseCoefficients; + } + +private: + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; + std::array naturalResponse; + std::array forcedResponse; +}; + +} // namespace tap::algorithms::filter + +#endif // TAPROOT_DISCRETE_FILTER_HPP_ \ No newline at end of file diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp index 71332bdd..3943d305 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp @@ -62,6 +62,13 @@ class Odometry2DInterface * @return The last time that odometry was computed (in microseconds). */ virtual uint32_t getLastComputedOdometryTime() const = 0; + + /** + * Resets odometry to initial pose. + */ + virtual void reset() = 0; + + virtual void overrideOdometryPosition(const float positionX, const float positionY) = 0; }; } // namespace tap::algorithms::odometry diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp index 372e1f23..868fe2ca 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp @@ -24,6 +24,7 @@ #include +#include "tap/architecture/clock.hpp" #include "tap/control/chassis/chassis_subsystem_interface.hpp" #include "modm/math/geometry/angle.hpp" @@ -80,4 +81,9 @@ void Odometry2DTracker::update() } } +void Odometry2DTracker::overrideOdometryPosition(const float positionX, const float positionY) +{ + location.setPosition(positionX, positionY); +} + } // namespace tap::algorithms::odometry diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp index d4bcd2dd..f2309507 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp @@ -79,6 +79,8 @@ class Odometry2DTracker : public Odometry2DInterface inline uint32_t getLastComputedOdometryTime() const final { return lastComputedOdometryTime; } + void overrideOdometryPosition(const float positionX, const float positionY); + private: ChassisWorldYawObserverInterface* chassisYawObserver; ChassisDisplacementObserverInterface* chassisDisplacementObserver; diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp new file mode 100644 index 00000000..cc9db3d3 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp @@ -0,0 +1,83 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_ANGULAR_VELOCITY_HPP_ +#define TAPROOT_ANGULAR_VELOCITY_HPP_ + +#include "tap/algorithms/cmsis_mat.hpp" + +namespace tap::algorithms::transforms +{ +class AngularVelocity +{ +public: + inline AngularVelocity(const float rollVel, const float pitchVel, const float yawVel) + : matrix_(skewMatFromAngVel(rollVel, pitchVel, yawVel)) + { + } + + /* rvalue reference */ + inline AngularVelocity(AngularVelocity&& other) : matrix_(std::move(other.matrix_)) {} + + /* Costly; use rvalue reference whenever possible */ + inline AngularVelocity(AngularVelocity& other) : matrix_(CMSISMat(other.matrix_)) {} + + /* Costly; use rvalue reference whenever possible */ + inline AngularVelocity(const CMSISMat<3, 3>& matrix) : matrix_(matrix) {} + + inline AngularVelocity(CMSISMat<3, 3>&& matrix) : matrix_(std::move(matrix)) {} + + /** + * @brief Get the roll velocity + */ + inline float getRollVelocity() const { return matrix_.data[0 * 3 + 2]; } + + /** + * @brief Get the pitch velocity + */ + inline float getPitchVelocity() const { return -matrix_.data[1 * 3 + 2]; } + + /** + * @brief Get the yaw velocity + */ + inline float getYawVelocity() const { return -matrix_.data[0 * 3 + 1]; } + + const inline CMSISMat<3, 3>& matrix() const { return matrix_; } + + /** + * Generates a 3x3 skew matrix from euler angle velocities (in radians/sec) + */ + inline static CMSISMat<3, 3> skewMatFromAngVel(const float wx, const float wy, const float wz) + { + return tap::algorithms::CMSISMat<3, 3>({0, -wz, wy, wz, 0, -wx, -wy, wx, 0}); + } + + friend class Transform; + friend class DynamicOrientation; + +private: + CMSISMat<3, 3> matrix_; +}; // class AngularVelocity +} // namespace tap::algorithms::transforms + +#endif // TAPROOT_ANGULAR_VELOCITY_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp new file mode 100644 index 00000000..3b6f9e6f --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp @@ -0,0 +1,133 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ +#ifndef TAPROOT_DYNAMIC_ORIENTATION_HPP_ +#define TAPROOT_DYNAMIC_ORIENTATION_HPP_ + +#include "tap/algorithms/cmsis_mat.hpp" + +#include "angular_velocity.hpp" +#include "orientation.hpp" + +namespace tap::algorithms::transforms +{ +class DynamicOrientation +{ +public: + inline DynamicOrientation( + const float roll, + const float pitch, + const float yaw, + const float rollVel, + const float pitchVel, + const float yawVel) + : orientation(Orientation::fromEulerAngles(roll, pitch, yaw)), + angularVelocity(AngularVelocity::skewMatFromAngVel(rollVel, pitchVel, yawVel)) + { + } + + inline DynamicOrientation(Orientation&& orientation, AngularVelocity&& angularVelocity) + : orientation(std::move(orientation.matrix_)), + angularVelocity(std::move(angularVelocity.matrix_)) + { + } + + inline DynamicOrientation(Orientation& orientation, AngularVelocity& angularVelocity) + : orientation(orientation.matrix_), + angularVelocity(angularVelocity.matrix_) + { + } + + inline DynamicOrientation( + const CMSISMat<3, 3>&& orientation, + const CMSISMat<3, 3>&& angularVelocity) + : orientation(std::move(orientation)), + angularVelocity(std::move(angularVelocity)) + { + } + + /* Costly; use rvalue reference whenever possible */ + inline DynamicOrientation( + const CMSISMat<3, 3>& orientation, + const CMSISMat<3, 3>& angularVelocity) + : orientation(orientation), + angularVelocity(angularVelocity) + { + } + + DynamicOrientation compose(const DynamicOrientation& other) const + { + return DynamicOrientation( + this->orientation * other.orientation, + this->angularVelocity + + this->orientation * other.angularVelocity * this->orientation.transpose()); + } + + DynamicOrientation inverse() const + { + return DynamicOrientation( + this->orientation.transpose(), + -(this->orientation.transpose() * this->angularVelocity * this->orientation)); + } + + inline Orientation getOrientation() const { return Orientation(orientation); } + + inline AngularVelocity getAngularVelocity() const { return AngularVelocity(angularVelocity); } + + /** + * Returns roll as values between [-pi, +pi]. + * + * If pitch is completely vertical (-pi / 2 or pi / 2) then roll and yaw are gimbal-locked. In + * this case, roll is taken to be 0. + */ + inline float roll() const { return atan2(orientation.data[7], orientation.data[8]); } + + inline float pitch() const { return asinf(-orientation.data[6]); } + + inline float yaw() const { return atan2(orientation.data[3], orientation.data[0]); } + + /** + * @brief Get the roll velocity + */ + inline float getRollVelocity() const { return angularVelocity.data[0 * 3 + 2]; } + + /** + * @brief Get the pitch velocity + */ + inline float getPitchVelocity() const { return -angularVelocity.data[1 * 3 + 2]; } + + /** + * @brief Get the yaw velocity + */ + inline float getYawVelocity() const { return -angularVelocity.data[0 * 3 + 1]; } + + friend class Transform; + +private: + CMSISMat<3, 3> orientation; + + CMSISMat<3, 3> angularVelocity; + +}; // class DynamicOrientation +} // namespace tap::algorithms::transforms + +#endif // TAPROOT_DYNAMIC_ORIENTATION_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp new file mode 100644 index 00000000..5f564aca --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp @@ -0,0 +1,131 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_DYNAMIC_POSITION_HPP_ +#define TAPROOT_DYNAMIC_POSITION_HPP_ + +#include "tap/algorithms/cmsis_mat.hpp" + +#include "position.hpp" +#include "vector.hpp" + +namespace tap::algorithms::transforms +{ +class DynamicPosition +{ +public: + inline DynamicPosition( + const float x, + const float y, + const float z, + const float vx, + const float vy, + const float vz, + const float ax, + const float ay, + const float az) + : position({x, y, z}), + velocity({vx, vy, vz}), + acceleration({ax, ay, az}) + { + } + + /* Costly; use rvalue reference whenever possible */ + inline DynamicPosition( + const CMSISMat<3, 1>& position, + const CMSISMat<3, 1>& velocity, + const CMSISMat<3, 1>& acceleration) + : position(position), + velocity(velocity), + acceleration(acceleration) + { + } + + inline DynamicPosition( + CMSISMat<3, 1>&& position, + CMSISMat<3, 1>&& velocity, + CMSISMat<3, 1>&& acceleration) + : position(std::move(position)), + velocity(std::move(velocity)), + acceleration(std::move(acceleration)) + { + } + + DynamicPosition operator+(const DynamicPosition& other) const + { + return DynamicPosition( + this->position + other.position, + this->velocity + other.velocity, + this->acceleration + other.acceleration); + } + + DynamicPosition operator-(const DynamicPosition& other) const + { + return DynamicPosition( + this->position - other.position, + this->velocity - other.velocity, + this->acceleration - other.acceleration); + } + + DynamicPosition operator-() const + { + return DynamicPosition(-this->position, -this->velocity, -this->acceleration); + } + + inline Position getPosition() const { return Position(position); } + + inline Vector getVelocity() const { return Vector(velocity); } + + inline Vector getAcceleration() const { return Vector(acceleration); } + + inline float x() const { return position.data[0]; } + + inline float y() const { return position.data[1]; } + + inline float z() const { return position.data[2]; } + + inline float vx() const { return velocity.data[0]; } + + inline float vy() const { return velocity.data[1]; } + + inline float vz() const { return velocity.data[2]; } + + inline float ax() const { return acceleration.data[0]; } + + inline float ay() const { return acceleration.data[1]; } + + inline float az() const { return acceleration.data[2]; } + + friend class Transform; + +private: + CMSISMat<3, 1> position; + + CMSISMat<3, 1> velocity; + + CMSISMat<3, 1> acceleration; + +}; // class DynamicPosition +} // namespace tap::algorithms::transforms + +#endif // TAPROOT_DYNAMIC_POSITION_HPP_ diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp index 61e3b412..bbe6cae5 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp @@ -26,7 +26,6 @@ #include "tap/algorithms/cmsis_mat.hpp" #include "tap/algorithms/math_user_utils.hpp" -// #include "tap/algorithms/euler_angles.hpp" namespace tap::algorithms::transforms { @@ -49,6 +48,11 @@ class Orientation inline Orientation(CMSISMat<3, 3>&& matrix) : matrix_(std::move(matrix)) {} + inline Orientation compose(const Orientation& other) const + { + return Orientation(this->matrix_ * other.matrix_); + } + /** * Returns roll as values between [-pi, +pi]. * @@ -63,7 +67,25 @@ class Orientation const inline CMSISMat<3, 3>& matrix() const { return matrix_; } + /** + * Generates a 3x3 rotation matrix from euler angles (in radians) + */ + static CMSISMat<3, 3> fromEulerAngles(const float roll, const float pitch, const float yaw) + { + return tap::algorithms::CMSISMat<3, 3>( + {cosf(yaw) * cosf(pitch), + (cosf(yaw) * sinf(pitch) * sinf(roll)) - (sinf(yaw) * cosf(roll)), + (cosf(yaw) * sinf(pitch) * cosf(roll)) + sinf(yaw) * sinf(roll), + sinf(yaw) * cosf(pitch), + sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll), + sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll), + -sinf(pitch), + cosf(pitch) * sinf(roll), + cosf(pitch) * cosf(roll)}); + } + friend class Transform; + friend class DynamicOrientation; private: CMSISMat<3, 3> matrix_; diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp index 227a8fa9..4fb495d6 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp @@ -78,6 +78,9 @@ class Position static float distance(const Position& a, const Position& b); + friend class Transform; + friend class DynamicPosition; + private: CMSISMat<3, 1> coordinates_; }; // class Position diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp index 73ba68c3..3f810e66 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp @@ -3,7 +3,7 @@ /*****************************************************************************/ /* - * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington + * Copyright (c) 2022-2024 Advanced Robotics at the University of Washington * * This file is part of Taproot. * @@ -26,70 +26,312 @@ namespace tap::algorithms::transforms { Transform::Transform(const Position& translation, const Orientation& rotation) - : translation(translation.coordinates()), + : dynamic(false), + translation(translation.coordinates()), + transVel({0, 0, 0}), + transAcc({0, 0, 0}), rotation(rotation.matrix()), - tRotation(rotation.matrix().transpose()) + tRotation(rotation.matrix().transpose()), + angVel({0, 0, 0, 0, 0, 0, 0, 0, 0}) { } Transform::Transform(Position&& translation, Orientation&& rotation) - : translation(std::move(translation.coordinates())), + : dynamic(false), + translation(std::move(translation.coordinates())), + transVel({0, 0, 0}), + transAcc({0, 0, 0}), rotation(std::move(rotation.matrix())), - tRotation(rotation.matrix().transpose()) + tRotation(rotation.matrix().transpose()), + angVel({0, 0, 0, 0, 0, 0, 0, 0, 0}) { } Transform::Transform(const CMSISMat<3, 1>& translation, const CMSISMat<3, 3>& rotation) - : translation(translation), + : dynamic(false), + translation(translation), + transVel({0, 0, 0}), + transAcc({0, 0, 0}), rotation(rotation), - tRotation(rotation.transpose()) + tRotation(rotation.transpose()), + angVel({0, 0, 0, 0, 0, 0, 0, 0, 0}) { } Transform::Transform(CMSISMat<3, 1>&& translation, CMSISMat<3, 3>&& rotation) + : dynamic(false), + translation(std::move(translation)), + transVel({0, 0, 0}), + transAcc({0, 0, 0}), + rotation(std::move(rotation)), + tRotation(rotation.transpose()), + angVel({0, 0, 0, 0, 0, 0, 0, 0, 0}) +{ +} + +Transform::Transform(float x, float y, float z, float rx, float ry, float rz) + : dynamic(false), + translation({x, y, z}), + transVel({0, 0, 0}), + transAcc({0, 0, 0}), + rotation(Orientation::fromEulerAngles(rx, ry, rz)), + tRotation(rotation.transpose()), + angVel({0, 0, 0, 0, 0, 0, 0, 0, 0}) +{ +} + +Transform::Transform( + const Position& translation, + const Orientation& rotation, + const Vector& velocity, + const Vector& acceleration, + const Vector& angularVelocity) + : translation(translation.coordinates()), + transVel(velocity.coordinates()), + transAcc(acceleration.coordinates()), + rotation(rotation.matrix()), + tRotation(rotation.matrix().transpose()), + angVel(AngularVelocity::skewMatFromAngVel( + angularVelocity.x(), + angularVelocity.y(), + angularVelocity.z())) +{ + checkDynamic(); +} + +Transform::Transform( + Position&& translation, + Orientation&& rotation, + Vector&& velocity, + Vector&& acceleration, + Vector&& angularVelocity) + : translation(std::move(translation.coordinates())), + transVel(std::move(velocity.coordinates())), + transAcc(std::move(acceleration.coordinates())), + rotation(std::move(rotation.matrix())), + tRotation(rotation.matrix().transpose()), + angVel(AngularVelocity::skewMatFromAngVel( + angularVelocity.x(), + angularVelocity.y(), + angularVelocity.z())) +{ + checkDynamic(); +} + +Transform::Transform( + const DynamicPosition& dynamicPosition, + const DynamicOrientation& dynamicOrientation) + : dynamic(true), + translation(dynamicPosition.position), + transVel(dynamicPosition.velocity), + transAcc(dynamicPosition.acceleration), + rotation(dynamicOrientation.orientation), + tRotation(rotation.transpose()), + angVel(dynamicOrientation.angularVelocity) +{ + checkDynamic(); +} + +Transform::Transform(DynamicPosition&& dynamicPosition, DynamicOrientation&& dynamicOrientation) + : dynamic(true), + translation(std::move(dynamicPosition.position)), + transVel(std::move(dynamicPosition.velocity)), + transAcc(std::move(dynamicPosition.acceleration)), + rotation(std::move(dynamicOrientation.orientation)), + tRotation(rotation.transpose()), + angVel(std::move(dynamicOrientation.angularVelocity)) +{ + checkDynamic(); +} + +Transform::Transform( + const CMSISMat<3, 1>& translation, + const CMSISMat<3, 3>& rotation, + const CMSISMat<3, 1>& velocity, + const CMSISMat<3, 1>& acceleration, + const CMSISMat<3, 3>& angularVelocity) + : translation(translation), + transVel(velocity), + transAcc(acceleration), + rotation(rotation), + tRotation(rotation.transpose()), + angVel(angularVelocity) +{ + checkDynamic(); +} + +Transform::Transform( + CMSISMat<3, 1>&& translation, + CMSISMat<3, 3>&& rotation, + CMSISMat<3, 1>&& velocity, + CMSISMat<3, 1>&& acceleration, + CMSISMat<3, 3>&& angularVelocity) : translation(std::move(translation)), + transVel(std::move(velocity)), + transAcc(std::move(acceleration)), rotation(std::move(rotation)), - tRotation(rotation.transpose()) + tRotation(rotation.transpose()), + angVel(std::move(angularVelocity)) { + checkDynamic(); } -Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw) +Transform::Transform( + float x, + float y, + float z, + float vx, + float vy, + float vz, + float ax, + float ay, + float az, + float rx, + float ry, + float rz, + float wx, + float wy, + float wz) : translation({x, y, z}), - rotation(fromEulerAngles(roll, pitch, yaw)), - tRotation(rotation.transpose()) + transVel({vx, vy, vz}), + transAcc({ax, ay, az}), + rotation(Orientation::fromEulerAngles(rx, ry, rz)), + tRotation(rotation.transpose()), + angVel(AngularVelocity::skewMatFromAngVel(wx, wy, wz)) { + checkDynamic(); } Position Transform::apply(const Position& position) const { - return Position(tRotation * (position.coordinates() - translation)); + return Position(tRotation * (position.coordinates_ - translation)); } Vector Transform::apply(const Vector& vector) const { - return Vector(tRotation * vector.coordinates()); + return Vector(tRotation * vector.coordinates_); +} + +DynamicPosition Transform::apply(const DynamicPosition& p) const +{ + CMSISMat<3, 1> pf = tRotation * (p.position - translation); + CMSISMat<3, 1> vf = tRotation * (p.velocity - transVel + angVel * (translation - p.position)); + CMSISMat<3, 1> af = + tRotation * (p.acceleration - transAcc + + angVel * (2 * (transVel - p.velocity) + angVel * (p.position - translation))); + return DynamicPosition(pf, vf, af); } Orientation Transform::apply(const Orientation& orientation) const { - return Orientation(tRotation * orientation.matrix()); + return Orientation(tRotation * orientation.matrix_); +} + +DynamicOrientation Transform::apply(const DynamicOrientation& dynamicOrientation) const +{ + return DynamicOrientation( + tRotation * dynamicOrientation.orientation, + tRotation * (dynamicOrientation.angularVelocity - angVel) * rotation); } Transform Transform::getInverse() const { // negative transposed rotation matrix times original position = new position - CMSISMat<3, 1> invTranslation = tRotation * translation; - invTranslation = -invTranslation; - return Transform(invTranslation, tRotation); + CMSISMat<3, 1> invTranslation = -tRotation * translation; + if (dynamic) + { + CMSISMat<3, 1> invVel = tRotation * (angVel * translation - transVel); + CMSISMat<3, 1> invAcc = + tRotation * (angVel * (2 * transVel - angVel * translation) - transAcc); + CMSISMat<3, 3> invAngVel = -tRotation * angVel * rotation; + return Transform(invTranslation, tRotation, invVel, invAcc, invAngVel); + } + else + { + return Transform(invTranslation, tRotation); + } } Transform Transform::compose(const Transform& second) const +{ + if (this->dynamic && second.dynamic) + { + CMSISMat<3, 3> newRot = this->rotation * second.rotation; + CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation; + CMSISMat<3, 1> newVel = this->transVel + + this->angVel * this->rotation * second.translation + + this->rotation * second.transVel; + CMSISMat<3, 1> newAcc = + this->transAcc + this->angVel * this->angVel * this->rotation * second.translation + + 2 * this->angVel * this->rotation * second.transVel + this->rotation * second.transAcc; + CMSISMat<3, 3> newAngVel = this->angVel + this->rotation * second.angVel * this->tRotation; + return Transform(newPos, newRot, newVel, newAcc, newAngVel); + } + else if (this->dynamic) + { + CMSISMat<3, 3> newRot = this->rotation * second.rotation; + CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation; + CMSISMat<3, 1> newVel = this->transVel + this->angVel * this->rotation * second.translation; + CMSISMat<3, 1> newAcc = + this->transAcc + this->angVel * this->angVel * this->rotation * second.translation; + CMSISMat<3, 3> newAngVel = this->angVel; + return Transform(newPos, newRot, newVel, newAcc, newAngVel); + } + else if (second.dynamic) + { + CMSISMat<3, 3> newRot = this->rotation * second.rotation; + CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation; + CMSISMat<3, 1> newVel = this->rotation * second.transVel; + CMSISMat<3, 1> newAcc = this->rotation * second.transAcc; + CMSISMat<3, 3> newAngVel = this->rotation * second.angVel * this->tRotation; + return Transform(newPos, newRot, newVel, newAcc, newAngVel); + } + + return composeStatic(second); +} + +Transform Transform::composeStatic(const Transform& second) const { CMSISMat<3, 3> newRot = this->rotation * second.rotation; CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation; return Transform(newPos, newRot); } +Transform Transform::projectForward(float dt) const +{ + if (!dynamic) + { + return Transform( + this->translation, + this->rotation, + this->transVel, + this->transAcc, + this->angVel); + } + + CMSISMat<3, 1> newPos = + this->translation + dt * this->transVel + 0.5f * dt * dt * this->transAcc; + CMSISMat<3, 1> newVel = this->transVel + dt * this->transAcc; + + float angVelMag = sqrt( + getRollVelocity() * getRollVelocity() + getPitchVelocity() * getPitchVelocity() + + getYawVelocity() * getYawVelocity()); + + if (compareFloatClose(angVelMag, 0, 1e-3)) + { + return Transform(newPos, this->rotation, newVel, this->transAcc, this->angVel); + } + + float theta = dt * angVelMag; + CMSISMat<3, 3> angVelNormalized = angVel / angVelMag; + CMSISMat<3, 3> velDt = CMSISMat<3, 3>(); + velDt.constructIdentityMatrix(); + velDt = velDt + sin(theta) * angVelNormalized + + (1 - cos(theta)) * angVelNormalized * angVelNormalized; + CMSISMat<3, 3> newRot = velDt * this->rotation; + return Transform(newPos, newRot, newVel, this->transAcc, this->angVel); +} + float Transform::getRoll() const { float jz = rotation.data[2 * 3 + 1]; @@ -97,12 +339,16 @@ float Transform::getRoll() const return atan2(jz, kz); } +float Transform::getRollVelocity() const { return angVel.data[0 * 3 + 2]; } + float Transform::getPitch() const { float iz = rotation.data[2 * 3 + 0]; return asinf(-iz); } +float Transform::getPitchVelocity() const { return -angVel.data[1 * 3 + 2]; } + float Transform::getYaw() const { float iy = rotation.data[1 * 3 + 0]; @@ -110,4 +356,6 @@ float Transform::getYaw() const return atan2(iy, ix); } +float Transform::getYawVelocity() const { return -angVel.data[0 * 3 + 1]; } + } // namespace tap::algorithms::transforms diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp index 6910bb75..237913a3 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp @@ -3,7 +3,7 @@ /*****************************************************************************/ /* - * Copyright (c) 2022-2023 Advanced Robotics at the University of Washington + * Copyright (c) 2022-2024 Advanced Robotics at the University of Washington * * This file is part of Taproot. * @@ -20,11 +20,16 @@ * You should have received a copy of the GNU General Public License * along with Taproot. If not, see . */ + #ifndef TAPROOT_TRANSFORM_HPP_ #define TAPROOT_TRANSFORM_HPP_ +#include "tap/algorithms/cmsis_mat.hpp" #include "tap/algorithms/math_user_utils.hpp" +#include "angular_velocity.hpp" +#include "dynamic_orientation.hpp" +#include "dynamic_position.hpp" #include "orientation.hpp" #include "position.hpp" #include "vector.hpp" @@ -32,24 +37,24 @@ namespace tap::algorithms::transforms { /** - Represents a transformation from one coordinate frame to another. - - A Transform from frame A to frame B defines a relationship between the two frames, such that a - spatial measurement in frame A can be represented equivalently in frame B by applying a - translational and rotational offset. This process is known as *applying* a transform. - - Transforms are specified as a translation and rotation of some "target" frame relative to some - "source" frame. The "translation" is the target frame's origin in source frame, and the - "rotation" is the target frame's orientation relative to the source frame's orientation. - - Conceptually, translations are applied "before" rotations. This means that the origin of the - target frame is entirely defined by the translation in the source frame, and the rotation serves - only to change the orientation of the target frame's axes relative to the source frame. - - Utilizes arm's CMSIS matrix operations. - - @param SOURCE represents the source frame of the transformation. - @param TARGET represents the target frame of the transformation. + * Represents a transformation from one coordinate frame to another. + * A Static Transform from frame A to frame B defines a relationship between the two frames, such + * that a spatial measurement in frame A can be represented equivalently in frame B by applying a + * translational and rotational offset. This process is known as *applying* a transform. + * + * Static Transforms are specified as a translation and rotation of some "follower" frame relative + * to some "base" frame. The "translation" is the follower frame's origin in base frame, and the + * "rotation" is the follower frame's orientation relative to the base frame's orientation. + * + * Conceptually, translations are applied "before" rotations. This means that the origin of the + * follower frame is entirely defined by the translation in the base frame, and the rotation serves + * only to change the orientation of the follower frame's axes relative to the base frame. + * + * A Dynamic Transform is an extension of a Static Transform that can store linear velocity, linear + * acceleration, and angular velocity. This class handles both, automatically determining whether it + * is static or dynamic. + * + * Utilizes ARM's CMSIS matrix operations. */ class Transform { @@ -70,67 +75,183 @@ class Transform /** * Constructs rotations using XYZ Euler angles, - * so rotations are applied in order of A, B, then C. + * so rotations are applied in order of rx, ry, then rz. * As an example, for an x-forward, z-up coordinate system, * this is in the order of roll, pitch, then yaw. * * @param x: Initial x-component of the translation. * @param y: Initial y-component of the translation. * @param z: Initial z-component of the translation. - * @param A: Initial rotation angle about the x-axis. - * @param B: Initial rotation angle about the y-axis. - * @param C: Initial rotation angle about the z-axis. + * @param rx: Initial rotation angle about the x-axis. + * @param ry: Initial rotation angle about the y-axis. + * @param rz: Initial rotation angle about the z-axis. */ - Transform(float x, float y, float z, float roll, float pitch, float yaw); + Transform(float x, float y, float z, float rx, float ry, float rz); - // TODO: template specialization for transform between identical frames?? /** - * Constructs an identity transform. + * @param translation Initial translation of this transformation. + * @param rotation Initial rotation of this transformation. + * @param velocity Translational velocity of this transformation. + * @param acceleration Translational acceleration of this transformation. + * @param angularVelocity Angular velocity pseudovector of this transformation. + */ + Transform( + const Position& translation, + const Orientation& rotation, + const Vector& velocity, + const Vector& acceleration, + const Vector& angularVelocity); + + /** + * @param translation Initial translation of this transformation. + * @param rotation Initial rotation of this transformation. + * @param velocity Translational velocity of this transformation. + * @param acceleration Translational acceleration of this transformation. + * @param angularVelocity Angular velocity pseudovector of this transformation. + */ + Transform( + Position&& translation, + Orientation&& rotation, + Vector&& velocity, + Vector&& acceleration, + Vector&& angularVelocity); + + /** + * @param rotation Initial rotation of this transformation. + * @param position Initial translation of this transformation. + */ + Transform(const DynamicPosition& dynamicPosition, const DynamicOrientation& dynamicOrientation); + Transform(DynamicPosition&& dynamicPosition, DynamicOrientation&& dynamicOrientation); + + /** + * @param translation Initial translation of this transformation. + * @param rotation Initial rotation of this transformation. + * @param velocity Translational velocity of this transformation. + * @param acceleration Translational acceleration of this transformation. + * @param angularVelocity Angular velocity skew symmetric matrix of this transformation. + */ + Transform( + const CMSISMat<3, 1>& translation, + const CMSISMat<3, 3>& rotation, + const CMSISMat<3, 1>& velocity, + const CMSISMat<3, 1>& acceleration, + const CMSISMat<3, 3>& angularVelocity); + + /** + * @param translation Initial translation of this transformation. + * @param rotation Initial rotation of this transformation. + * @param velocity Translational velocity of this transformation. + * @param acceleration Translational acceleration of this transformation. + * @param angularVelocity Angular velocity skew symmetric matrix of this transformation. + */ + Transform( + CMSISMat<3, 1>&& translation, + CMSISMat<3, 3>&& rotation, + CMSISMat<3, 1>&& velocity, + CMSISMat<3, 1>&& acceleration, + CMSISMat<3, 3>&& angularVelocity); + + /** + * Constructs rotations using XYZ Euler angles, + * so rotations are applied in order of rx, ry, then rx. + * + * @param x: Initial x-component of the translation. + * @param y: Initial y-component of the translation. + * @param z: Initial z-component of the translation. + * @param vx: Initial x-component of the translational velocity. + * @param vy: Initial y-component of the translational velocity. + * @param vz: Initial z-component of the translational velocity. + * @param ax: Initial x-component of the translational acceleration. + * @param ay: Initial y-component of the translational acceleration. + * @param az: Initial z-component of the translational acceleration. + * @param rx: Initial rotation angle about the x-axis. + * @param ry: Initial rotation angle about the y-axis. + * @param rz: Initial rotation angle about the z-axis. + * @param wx: Initial angular velocity about the x-axis. + * @param wy: Initial angular velocity about the y-axis. + * @param wz: Initial angular velocity about the z-axis. + */ + Transform( + float x, + float y, + float z, + float vx, + float vy, + float vz, + float ax, + float ay, + float az, + float rx, + float ry, + float rz, + float wx, + float wy, + float wz); + + /** + * @brief Constructs an identity transform. */ static inline Transform identity() { return Transform(0., 0., 0., 0., 0., 0.); } /** - * Apply this transform to a position. + * @brief Apply this transform to a position. * - * @param[in] position Position in source frame. - * @return Position in target frame. + * @param[in] position Position in base frame. + * @return Position in follower frame. */ Position apply(const Position& position) const; /** - * Rotates a vector in the source frame to a vector in the target frame. + * @brief Rotates a vector in the base frame to a vector in the follower frame. * * Intended to be used for things like velocities and accelerations which represent the * difference between two positions in space, since both positions get translated the same way, * causing the translation to cancel out. * - * @param vector Vector as read by source frame. - * @return Vector in target frame's basis. + * @note Only accurate for static transforms! + * + * @param vector Vector as read by base frame. + * @return Vector in follower frame's basis. */ Vector apply(const Vector& vector) const; /** - * + * @brief Brings a dynamic position in the base frame to one in the follower frame. + */ + DynamicPosition apply(const DynamicPosition& dynamicPosition) const; + + /** + * @brief Brings an orientation in the base frame to one in the follower frame. */ Orientation apply(const Orientation& orientation) const; /** - * Updates the translation of the current transformation matrix. + * @brief Brings a dynamic orientation in the base frame to one in the follower frame. + */ + DynamicOrientation apply(const DynamicOrientation& dynamicOrientation) const; + + /** + * @brief Updates the translation of the current transformation matrix. * - * @param newTranslation updated position of target in source frame. + * @param newTranslation updated position of follower in base frame. */ inline void updateTranslation(const Position& newTranslation) { this->translation = newTranslation.coordinates(); } + /** + * @brief Updates the translation of the current transformation matrix. + * + * @param newTranslation updated position of follower in base frame. + */ inline void updateTranslation(Position&& newTranslation) { this->translation = std::move(newTranslation.coordinates()); } /** - * Updates the translation of the current transformation matrix. + * @brief Updates the translation of the current transformation matrix. * * @param x new translation x-component. * @param y new translation y-component. @@ -142,9 +263,33 @@ class Transform } /** - * Updates the rotation of the current transformation matrix. + * @brief Updates the translation of the current transformation matrix. + * + * @param newTranslation updated position of follower in base frame. + */ + inline void updateTranslation(const DynamicPosition& newTranslation) + { + this->translation = newTranslation.position; + this->transVel = newTranslation.velocity; + this->transAcc = newTranslation.acceleration; + } + + /** + * @brief Updates the translation of the current transformation matrix. * - * @param newRotation updated orienation of target frame in source frame. + * @param newTranslation updated position of follower in base frame. + */ + inline void updateTranslation(DynamicPosition&& newTranslation) + { + this->translation = std::move(newTranslation.position); + this->transVel = std::move(newTranslation.velocity); + this->transAcc = std::move(newTranslation.acceleration); + } + + /** + * @brief Updates the rotation of the current transformation matrix. + * + * @param newRotation updated orientation of follower frame in base frame. */ inline void updateRotation(const Orientation& newRotation) { @@ -152,6 +297,11 @@ class Transform this->tRotation = this->rotation.transpose(); } + /** + * @brief Updates the rotation of the current transformation matrix. + * + * @param newRotation updated orientation of follower frame in base frame. + */ inline void updateRotation(Orientation&& newRotation) { this->rotation = std::move(newRotation.matrix()); @@ -159,7 +309,7 @@ class Transform } /** - * Updates the rotation of the current transformation matrix. + * @brief Updates the rotation of the current transformation matrix. * Takes rotation angles in the order of roll->pitch->yaw. * * @param roll updated rotation angle about the x-axis. @@ -172,58 +322,303 @@ class Transform this->tRotation = this->rotation.transpose(); } + /** + * @brief Updates the rotation of the current transformation matrix. + * + * @param newRotation updated orientation of follower frame in base frame. + */ + inline void updateRotation(const DynamicOrientation& newRotation) + { + this->rotation = newRotation.orientation; + this->tRotation = this->rotation.transpose(); + this->angVel = newRotation.angularVelocity; + } + + /** + * @brief Updates the rotation of the current transformation matrix. + * + * @param newRotation updated orientation of follower frame in base frame. + */ + inline void updateRotation(DynamicOrientation&& newRotation) + { + this->rotation = std::move(newRotation.orientation); + this->tRotation = this->rotation.transpose(); + this->angVel = std::move(newRotation.angularVelocity); + } + + /** + * @brief Updates the velocity of the current transform. + * + * @param newVelocity updated velocity of follower in base frame. + */ + inline void updateVelocity(const Vector& newVelocity) + { + this->transVel = newVelocity.coordinates(); + checkDynamic(); + } + + /** + * @brief Updates the velocity of the current transform. + * + * @param newVelocity updated velocity of follower in base frame. + */ + inline void updateVelocity(Vector&& newVelocity) + { + this->transVel = std::move(newVelocity.coordinates()); + checkDynamic(); + } + + /** + * @brief Updates the velocity of the current transform. + * + * @param vx new velocity x-component. + * @param vy new velocity y-component. + * @param vz new velocity z-component. + */ + inline void updateVelocity(float vx, float vy, float vz) + { + this->transVel = CMSISMat<3, 1>({vx, vy, vz}); + checkDynamic(); + } + + /** + * @brief Updates the acceleration of the current transform. + * + * @param updateAcceleration updated acceleration of follower in base frame. + */ + inline void updateAcceleration(const Vector& newAcceleration) + { + this->transVel = newAcceleration.coordinates(); + checkDynamic(); + } + + /** + * @brief Updates the acceleration of the current transform. + * + * @param updateAcceleration updated acceleration of follower in base frame. + */ + inline void updateAcceleration(Vector&& newAcceleration) + { + this->transVel = std::move(newAcceleration.coordinates()); + checkDynamic(); + } + + /** + * @brief Updates the acceleration of the current transform. + * + * @param ax new acceleration x-component. + * @param ay new acceleration y-component. + * @param az new acceleration z-component. + */ + inline void updateAcceleration(float ax, float ay, float az) + { + this->transAcc = CMSISMat<3, 1>({ax, ay, az}); + checkDynamic(); + } + + /** + * @brief Updates the angular velocity of the current transform. + * + * @param updateAngularVelocity updated angular velocity of follower in base frame. + */ + inline void updateAngularVelocity(const Vector& newAngularVelocity) + { + this->angVel = AngularVelocity::skewMatFromAngVel( + newAngularVelocity.x(), + newAngularVelocity.y(), + newAngularVelocity.z()); + checkDynamic(); + } + + /** + * @brief Updates the angular velocity of the current transform. + * + * @param updateAngularVelocity updated angular velocity of follower in base frame. + */ + inline void updateAngularVelocity(Position&& newAngularVelocity) + { + this->angVel = AngularVelocity::skewMatFromAngVel( + newAngularVelocity.x(), + newAngularVelocity.y(), + newAngularVelocity.z()); + checkDynamic(); + } + + /** + * @brief Updates the angular velocity of the current transform. + * + * @param ax new angular velocity x-component. + * @param ay new angular velocity y-component. + * @param az new angular velocity z-component. + */ + inline void updateAngularVelocity(float vr, float vp, float vy) + { + this->angVel = AngularVelocity::skewMatFromAngVel(vr, vp, vy); + checkDynamic(); + } + /** * @return Inverse of this Transform. + * + * @note This is only instantaneously correct for dynamic transforms; It can no longer be + * projected forward in time and behave the same way as the original. This is due to the now + * reversed translation-rotation that would be required to truly mimic the motion of the + * original. Ex: Consider a dynamic transform with only non-zero translation and angular + * velocity. Projecting this forward will cause the follower frame to rotate around its origin. + * Intuitively, one would expect the inverted transform to have its follower frame rotate around + * the base frame origin. However, this circular translation can only be approximated here with + * translational velocity/acceleration. The true inverse would need to be the composition of a + * rotation *then* a translation. */ Transform getInverse() const; /** - * Returns the composed transformation of the given transformations. - * @return Transformation from frame A to frame C. + * @brief Returns the composed transformation of the given transformations. + * + * @return Transformation from this transform's base frame to `second`'s follower frame. */ Transform compose(const Transform& second) const; + /** + * @brief Returns the composed transformation of the given transformations, ignoring any time + * derivatives. + * + * @return Static transformation from this transform's base frame to `second`'s + * follower frame. + */ + Transform composeStatic(const Transform& second) const; + + /** + * @brief Projects this transform forward in time according to its translational + * velocity/acceleration and angular velocity. + * + * @param dt Seconds to project forward (can be negative) + * @return Projected transform + */ + Transform projectForward(float dt) const; + /* Getters */ inline Position getTranslation() const { return Position(translation); }; + inline Vector getVelocity() const { return Vector(transVel); }; + + inline Vector getAcceleration() const { return Vector(transAcc); }; + + inline DynamicPosition getDynamicTranslation() const + { + return DynamicPosition(translation, transVel, transAcc); + }; + inline Orientation getRotation() const { return Orientation(rotation); } + inline Vector getAngularVel() const + { + return Vector(getRollVelocity(), getPitchVelocity(), getYawVelocity()); + } + + inline DynamicOrientation getDynamicOrientation() const + { + return DynamicOrientation(rotation, angVel); + }; + /** - * Get the roll of this transformation + * @brief Get the roll of this transformation */ float getRoll() const; /** - * Get the pitch of this transformation + * @brief Get the pitch of this transformation */ float getPitch() const; /** - * Get the yaw of this transformation + * @brief Get the yaw of this transformation */ float getYaw() const; /** - * Get the x-component of this transform's translation + * @brief Get the roll velocity of this transformation + */ + float getRollVelocity() const; + + /** + * @brief Get the pitch velocity of this transformation + */ + float getPitchVelocity() const; + + /** + * @brief Get the yaw velocity of this transformation + */ + float getYawVelocity() const; + + /** + * @brief Get the x-component of this transform's translation */ inline float getX() const { return this->translation.data[0]; } /** - * Get the y-component of this transform's translation + * @brief Get the y-component of this transform's translation */ inline float getY() const { return this->translation.data[1]; } /** - * Get the z-component of this transform's translation + * @brief Get the z-component of this transform's translation */ inline float getZ() const { return this->translation.data[2]; } + /** + * @brief Get the x-component of this transform's linear velocity + */ + inline float getXVel() const { return this->transVel.data[0]; } + + /** + * @brief Get the y-component of this transform's linear velocity + */ + inline float getYVel() const { return this->transVel.data[1]; } + + /** + * @brief Get the z-component of this transform's linear velocity + */ + inline float getZVel() const { return this->transVel.data[2]; } + + /** + * @brief Get the x-component of this transform's linear acceleration + */ + inline float getXAcc() const { return this->transAcc.data[0]; } + + /** + * @brief Get the y-component of this transform's linear acceleration + */ + inline float getYAcc() const { return this->transAcc.data[1]; } + + /** + * @brief Get the z-component of this transform's linear acceleration + */ + inline float getZAcc() const { return this->transAcc.data[2]; } + + /** + * @brief Whether there are any non-zero derivatives. + */ + inline bool isDynamic() const { return dynamic; } + private: + bool dynamic{true}; + /** * Translation vector. */ CMSISMat<3, 1> translation; + /** + * Translational velocity vector. + */ + CMSISMat<3, 1> transVel; + + /** + * Translational acceleration vector. + */ + CMSISMat<3, 1> transAcc; + /** * Rotation matrix. */ @@ -236,6 +631,29 @@ class Transform * The transpose of a rotation is its inverse. */ CMSISMat<3, 3> tRotation; + + /** + * Angular velocity skew matrix. + */ + CMSISMat<3, 3> angVel; + + inline void checkDynamic() + { + dynamic = false; + + dynamic |= + !(compareFloatClose(getXVel(), 0, 1e-5) && compareFloatClose(getYVel(), 0, 1e-5) && + compareFloatClose(getZVel(), 0, 1e-5)); + + dynamic |= + !(compareFloatClose(getXAcc(), 0, 1e-5) && compareFloatClose(getYAcc(), 0, 1e-5) && + compareFloatClose(getZAcc(), 0, 1e-5)); + + dynamic |= + !(compareFloatClose(getRollVelocity(), 0, 1e-5) && + compareFloatClose(getPitchVelocity(), 0, 1e-5) && + compareFloatClose(getYawVelocity(), 0, 1e-5)); + } }; // class Transform } // namespace tap::algorithms::transforms diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp index e411e753..6cf19c5f 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp @@ -31,10 +31,4 @@ inline Vector Vector::operator+(const Position& other) const { return Vector(this->coordinates_ + other.coordinates()); } - -inline Vector Vector::operator+(const Vector& other) const -{ - return Vector(this->coordinates_ + other.coordinates_); -} - } // namespace tap::algorithms::transforms diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp index d4bb2df9..79720d1e 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp @@ -40,7 +40,10 @@ class Vector Vector(const Vector& other) : coordinates_(CMSISMat(other.coordinates_)) {} - Vector(CMSISMat<3, 1>& coordinates) : coordinates_(CMSISMat(coordinates)) {} + /** + * Costly copy constructor + */ + Vector(const CMSISMat<3, 1>& coordinates) : coordinates_(CMSISMat(coordinates)) {} Vector(CMSISMat<3, 1>&& coordinates) : coordinates_(std::move(coordinates)) {} @@ -56,10 +59,18 @@ class Vector return *this; } - inline Vector operator+(const Vector& other) const; - inline Vector operator+(const Position& other) const; + inline Vector operator+(const Vector& other) const + { + return Vector(this->coordinates_ + other.coordinates_); + } + + inline Vector operator-(const Vector& other) const + { + return Vector(this->coordinates_ - other.coordinates_); + } + inline Vector operator*(const float scale) const { return Vector(this->coordinates_ * scale); } inline static float dot(const Vector& a, const Vector& b) @@ -75,6 +86,9 @@ class Vector inline float magnitude() const { return sqrt(dot(*this, *this)); } + friend class Transform; + friend class DynamicPosition; + private: CMSISMat<3, 1> coordinates_; }; // class Vector diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp index e040f579..bc7c2c02 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp @@ -128,15 +128,9 @@ void WrappedFloat::shiftBounds(const float shiftMagnitude) void WrappedFloat::wrapValue() { float oldValue = wrapped; - if (oldValue < lowerBound) - { - this->wrapped = upperBound + fmodf(oldValue - upperBound, upperBound - lowerBound); - } - else if (oldValue >= upperBound) - { - this->wrapped = lowerBound + fmodf(oldValue - lowerBound, upperBound - lowerBound); - } - this->revolutions += floor((oldValue - lowerBound) / (upperBound - lowerBound)); + float dRev = floor((oldValue - lowerBound) / (upperBound - lowerBound)); + this->revolutions += dRev; + this->wrapped = oldValue - (upperBound - lowerBound) * dRev; } float WrappedFloat::limitValue( diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp index 57b27049..aff4eb5b 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp @@ -53,7 +53,7 @@ class WrappedFloat /** * @param[in] value: value to initialize with (doesn't have to be wrapped) * @param[in] lowerBound: lower wrapping bound, must be less than `upperBound` - * @param[in] lowerBound: upper wrapping bound, must be higher than `lowerBound` + * @param[in] upperBound: upper wrapping bound, must be higher than `lowerBound` */ WrappedFloat(float value, float lowerBound, float upperBound); diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp index eb01492a..33579f2c 100644 --- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp +++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp @@ -56,12 +56,30 @@ void CanRxHandler::attachReceiveHandler( CanRxListener* const canRxListener, CanRxListener** messageHandlerStore) { - uint16_t id = lookupTableIndexForCanId(canRxListener->canIdentifier); + uint16_t bin = binIndexForCanId(canRxListener->canIdentifier); - modm_assert(id < NUM_CAN_IDS, "CAN", "RX listener id out of bounds", 1); - modm_assert(messageHandlerStore[id] == nullptr, "CAN", "overloading", 1); + if (messageHandlerStore[bin] == nullptr) + { + messageHandlerStore[bin] = canRxListener; + } + else + { + CanRxListener* node = messageHandlerStore[bin]; - messageHandlerStore[id] = canRxListener; + do + { + if (node->canIdentifier == canRxListener->canIdentifier) + { + RAISE_ERROR(drivers, "overloading can rx listener"); + return; + } + + if (node->next == nullptr) break; + node = node->next; + } while (true); + + node->next = canRxListener; + } } void CanRxHandler::pollCanData() @@ -85,17 +103,17 @@ void CanRxHandler::processReceivedCanData( const modm::can::Message& rxMessage, CanRxListener* const* messageHandlerStore) { - uint16_t id = lookupTableIndexForCanId(rxMessage.getIdentifier()); + uint16_t bin = binIndexForCanId(rxMessage.getIdentifier()); - if (id >= NUM_CAN_IDS) + CanRxListener* listener = messageHandlerStore[bin]; + while (listener != nullptr && listener->canIdentifier != rxMessage.identifier) { - RAISE_ERROR(drivers, "Invalid can id received"); - return; + listener = listener->next; } - if (messageHandlerStore[id] != nullptr) + if (listener != nullptr) { - messageHandlerStore[id]->processMessage(rxMessage); + listener->processMessage(rxMessage); } } @@ -115,15 +133,31 @@ void CanRxHandler::removeReceiveHandler( const CanRxListener& canRxListener, CanRxListener** messageHandlerStore) { - int id = lookupTableIndexForCanId(canRxListener.canIdentifier); + int bin = binIndexForCanId(canRxListener.canIdentifier); - if (id >= NUM_CAN_IDS) + if (messageHandlerStore[bin] == nullptr) { - RAISE_ERROR(drivers, "index out of bounds"); + RAISE_ERROR(drivers, "listener not in handler storage"); return; } - messageHandlerStore[id] = nullptr; + if (messageHandlerStore[bin]->canIdentifier == canRxListener.canIdentifier) + { + messageHandlerStore[bin] = messageHandlerStore[bin]->next; + } + else + { + CanRxListener* node = messageHandlerStore[bin]; + while (node->next != nullptr) + { + if (node->next->canIdentifier == canRxListener.canIdentifier) + { + node->next = node->next->next; + return; + } + } + RAISE_ERROR(drivers, "listener not in handler storage"); + } } } // namespace tap::can diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp index 8e6683c3..27e8eea8 100644 --- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp +++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp @@ -65,10 +65,9 @@ class CanRxListener; * pollCanData function be called at a very high frequency, * so call this in a high frequency thread. * - * @note The CAN handler can handle 64 CAN ids between [`0x1E4`, `0x224`). In the middle of this - * range, CAN ids [`0x201`, `0x20B`] are used by the `DjiMotor` objects to receive data from - * DJI branded motors. If you would like to define your own protocol, it is recommended to - * avoid avoid using CAN ids in this range. + * @note CAN ids [`0x201`, `0x20B`] are used by the `DjiMotor` objects to receive + * data from DJI branded motors. If you would like to define your own protocol, it + * is recommended to avoid avoid using CAN ids in this range. * @note the DjiMotor driver reserves `0x1FF` and `0x200` for commanding motors, * and thus you should not attach listeners for these ids. * @@ -79,27 +78,22 @@ class CanRxListener; class CanRxHandler { public: - static constexpr uint16_t MIN_CAN_ID = 0x1E4; - static constexpr uint16_t NUM_CAN_IDS = 64; - static constexpr uint16_t MAX_CAN_ID = MIN_CAN_ID + NUM_CAN_IDS; + /** + * The number of "hash" bins to store listeners in. `8` is choosen as most users will only have + * DJI Motors on their can bus, reducing any unnecessary overhead storing empty bins. The bus + * also become saturated near this limit, so the performance hit of a linked list traversal is + * minimal. + */ + static constexpr uint8_t CAN_BINS = 8; CanRxHandler(Drivers* drivers); mockable ~CanRxHandler() = default; DISALLOW_COPY_AND_ASSIGN(CanRxHandler) /** - * Given a CAN identifier, returns the "normalized" id between [0, NUM_CAN_IDS), or a - * value >= NUM_CAN_IDS if the canId is outside the range specified. + * Given a CAN identifier, returns the bin index between [0, NUM_CAN_IDS) for the identifier. */ - static inline uint16_t lookupTableIndexForCanId(uint16_t canId) - { - if (canId < MIN_CAN_ID) - { - return NUM_CAN_IDS; - } - - return canId - MIN_CAN_ID; - } + static inline uint16_t binIndexForCanId(uint16_t canId) { return canId % CAN_BINS; } /** * Call this function to add a CanRxListener to the list of CanRxListener's @@ -110,12 +104,11 @@ class CanRxHandler * store listeners may or not be properly allocated if you do and * undefined behavior will follow. * @note if you attempt to add a listener with an identifier identical to - * something already in the `CanRxHandler`, an error is thrown and - * the handler does not add the listener. + * something already in the `CanRxHandler`, an error will be added to the + * `ErrorController` and the handler does not add the listener. * @see `CanRxListener` * * @param[in] listener the listener to be attached ot the handler. - * @return `true` if listener successfully added, `false` otherwise. */ mockable void attachReceiveHandler(CanRxListener* const listener); @@ -146,13 +139,13 @@ class CanRxHandler * Stores pointers to the `CanRxListeners` for CAN 1, referenced when * a new message is received. */ - CanRxListener* messageHandlerStoreCan1[NUM_CAN_IDS]; + CanRxListener* messageHandlerStoreCan1[CAN_BINS]; /** * Stores pointers to the `CanRxListeners` for CAN 2, referenced when * a new message is received. */ - CanRxListener* messageHandlerStoreCan2[NUM_CAN_IDS]; + CanRxListener* messageHandlerStoreCan2[CAN_BINS]; #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) public: diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp index f1c0d618..26b55bf7 100644 --- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp +++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp @@ -39,6 +39,7 @@ namespace tap class Drivers; namespace can { +class CanRxHandler; /** * A class that when extended allows you to interface with the `can_rx_handler`. * @@ -88,6 +89,8 @@ namespace can */ class CanRxListener { + friend class CanRxHandler; + public: /** * Construct a new CanRxListener, must specify the can identifier @@ -135,6 +138,14 @@ class CanRxListener const CanBus canBus; Drivers* drivers; + +#ifndef ENV_UNIT_TESTS +private: +#endif + /** + * The next CanRxListener in the linked list for lookups. + */ + CanRxListener* next = nullptr; }; // class CanRxListener } // namespace can diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp new file mode 100644 index 00000000..e4cdcb1d --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp @@ -0,0 +1,65 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_ENCODER_INTERFACE_HPP_ +#define TAPROOT_ENCODER_INTERFACE_HPP_ + +#include "tap/algorithms/wrapped_float.hpp" + +namespace tap::encoder +{ +class EncoderInterface +{ +public: + /** + * Initialize the encoder. For instance: attaching a can receiver. + */ + virtual void initialize() = 0; + /** + * Returns true when the encoder is online and reporting valid values. + */ + virtual bool isOnline() const = 0; + /** + * Zeros the encoder to its current position. All encoder positions will be based from this + * position. + */ + virtual void resetEncoderValue() = 0; + /** + * Gets the current position of the encoder, in a wrapped float on the range [0, M_TWOPI) + */ + virtual tap::algorithms::WrappedFloat getPosition() const = 0; + /** + * Gets the current velocity reported by the encoder. Returned in a value of radians / second + */ + virtual float getVelocity() const = 0; + /** + * Aligns this encoder to another encoder so that their positions are equal. + * If the two encoders are mechanically linked, they would then continue to report the same + * position. + */ + virtual void alignWith(EncoderInterface* other) = 0; +}; + +} // namespace tap::encoder + +#endif // TAPROOT_ENCODER_INTERFACE_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp new file mode 100644 index 00000000..a5e30cae --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp @@ -0,0 +1,203 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_MULTI_ENCODER_HPP_ +#define TAPROOT_MULTI_ENCODER_HPP_ + +#include + +#include "tap/util_macros.hpp" + +#include "modm/architecture/interface/assert.hpp" + +#include "encoder_interface.hpp" + +namespace tap::encoder +{ +/** + * A way to combine multiple encoders into one functional unit. The first encoder in the array + * *must* not be null, as it is the primary encoder. All other values can be null. This encoder + * uses the primary encoder as an initial source of truth. This allows the other encoders to align + * themselves to it, and then have the primary encoder disconnect with the system still working. + * This encoder is not valid until the primary encoder comes online, and stays online until either + * all synced encoders disconnect, or the primary encoder is offline and no other encoders have + * been synced. The positions returned by this encoder are averaged between all synced and online + * encoders. + */ +template +class MultiEncoder : public EncoderInterface +{ +public: + MultiEncoder(std::array encoders) + : encoders(encoders), + seenEncoders(0) + { + modm_assert(this->encoders[0] != nullptr, "MultiEncoder", "nullptr first encoder"); + } + + void initialize() override + { + for (EncoderInterface*& encoder : this->encoders) + { + if (encoder != nullptr) + { + encoder->initialize(); + } + } + } + + bool isOnline() const override + { + const_cast*>(this)->syncEncoders(); + + for (uint32_t i = 0; i < COUNT; i++) + { + if (this->validEncoder(i)) + { + return true; + } + } + + return false; + } + + tap::algorithms::WrappedFloat getPosition() const override + { + const_cast*>(this)->syncEncoders(); + int onlineEncoders = 0; + float position = 0; + + for (uint32_t i = 0; i < COUNT; i++) + { + if (this->validEncoder(i)) + { + position += this->encoders[i]->getPosition().getUnwrappedValue(); + onlineEncoders += 1; + } + } + + return tap::algorithms::WrappedFloat( + onlineEncoders == 0 ? 0 : position / onlineEncoders, + 0, + static_cast(M_TWOPI)); + } + + float getVelocity() const override + { + const_cast*>(this)->syncEncoders(); + int onlineEncoders = 0; + float velocity = 0; + + for (uint32_t i = 0; i < COUNT; i++) + { + if (this->validEncoder(i)) + { + velocity += this->encoders[i]->getVelocity(); + onlineEncoders += 1; + } + } + + return onlineEncoders == 0 ? 0 : velocity / onlineEncoders; + }; + + void resetEncoderValue() override + { + this->syncEncoders(); + + for (EncoderInterface*& encoder : this->encoders) + { + if (encoder != nullptr) + { + encoder->resetEncoderValue(); + } + } + } + + void alignWith(EncoderInterface* other) override + { + this->syncEncoders(); + + for (EncoderInterface*& encoder : this->encoders) + { + if (encoder != nullptr) + { + encoder->alignWith(other); + } + } + } + + DISALLOW_COPY_AND_ASSIGN(MultiEncoder) + +private: + std::array encoders; + uint32_t seenEncoders; + + void syncEncoders() + { + // The primary encoder *must* be online before syncing anything to it + if (this->encoders[0]->isOnline()) + { + for (uint32_t i = 1; i < COUNT; i++) + { + bool online = this->encoders[i] != nullptr && this->encoders[i]->isOnline(); + if (online && !this->seenEncoder(i)) + { + // Sync the newly discovered encoder to the primary encoder + this->seenEncoders |= 1 << i; + encoders[i]->alignWith(encoders[0]); + } + else if (validEncoder(i) && !seenEncoder(0)) + { + // Primary encoder disappeared but came back, resync it. + this->seenEncoders |= 1; + encoders[0]->alignWith(encoders[i]); + } + else if (!online) + { + // This encoder disappeared, remove it + this->seenEncoders &= ~(1 << i); + } + } + this->seenEncoders |= 1; + } + else + { + this->seenEncoders &= ~(1); + } + } + + inline bool validEncoder(uint32_t index) const + { + return this->seenEncoder(index) && this->encoders[index] != nullptr && + this->encoders[index]->isOnline(); + } + + inline bool seenEncoder(uint32_t index) const + { + return ((this->seenEncoders) & (1 << index)) != 0; + } +}; + +} // namespace tap::encoder + +#endif // TAPROOT_MULTI_ENCODER_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp new file mode 100644 index 00000000..bb8c36fe --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp @@ -0,0 +1,128 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "wrapped_encoder.hpp" + +#include "tap/algorithms/math_user_utils.hpp" +#include "tap/architecture/clock.hpp" + +namespace tap +{ +namespace encoder +{ +WrappedEncoder::WrappedEncoder( + bool isInverted, + uint32_t encoderResolution, + float gearRatio, + uint32_t encoderHomePosition) + : encoder(tap::algorithms::WrappedFloat(0, 0, encoderResolution)), + position(tap::algorithms::Angle(0)), + inverted(isInverted), + encoderResolution(encoderResolution), + gearRatio(gearRatio), + encoderHomePosition(tap::algorithms::WrappedFloat(encoderHomePosition, 0, encoderResolution)), + pastPosition(tap::algorithms::Angle(0)), + lastUpdateTime(0), + deltaTime(0) +{ +} + +void WrappedEncoder::resetEncoderValue() +{ + encoderHomePosition = encoder + encoderHomePosition; + encoder.setUnwrappedValue(0); + pastPosition -= position; + position.setUnwrappedValue(0); +} + +tap::algorithms::WrappedFloat WrappedEncoder::getPosition() const +{ +#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) + return tap::algorithms::Angle( + this->getEncoder().getUnwrappedValue() * static_cast(M_TWOPI) / encoderResolution * + gearRatio); +#else + return position; +#endif +} + +float WrappedEncoder::getVelocity() const +{ + if (deltaTime == 0) + { + return 0; + } + + return (position - pastPosition).getUnwrappedValue() / deltaTime * 1'000'000; +} + +void WrappedEncoder::alignWith(EncoderInterface* other) +{ + tap::algorithms::WrappedFloat positionDifference = other->getPosition() - position; + float offset = positionDifference.getUnwrappedValue() / static_cast(M_TWOPI) * + encoderResolution / gearRatio; + this->encoderHomePosition += offset; + this->encoder += offset; + this->position = other->getPosition(); + this->pastPosition += positionDifference; +} + +void WrappedEncoder::updateEncoderValue(uint32_t encoderActual) +{ + // invert motor if necessary + encoderActual = inverted ? encoderResolution - 1 - encoderActual : encoderActual; + + int32_t encoderRelativeToHome = + (int32_t)encoderActual - (int32_t)encoderHomePosition.getWrappedValue(); + + uint32_t newEncWrapped = encoderRelativeToHome < 0 + ? (int32_t)encoderResolution + encoderRelativeToHome + : encoderRelativeToHome; + + if (lastUpdateTime == 0) // The first time we get a value, want it to always be positive + { + encoder = tap::algorithms::WrappedFloat(newEncWrapped, 0, encoderResolution); + } + else + { + encoder += encoder.minDifference(newEncWrapped); + } + + uint32_t time = tap::arch::clock::getTimeMicroseconds(); + if (time < lastUpdateTime) + { + deltaTime = time + ((0xFFFFFFFFul) - lastUpdateTime); + } + else + { + deltaTime = time - lastUpdateTime; + } + lastUpdateTime = time; + + pastPosition = position; + position.setUnwrappedValue( + encoder.getUnwrappedValue() * static_cast(M_TWOPI) / encoderResolution * gearRatio); +} +} // namespace encoder + +} // namespace tap diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp new file mode 100644 index 00000000..fd35eeb2 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp @@ -0,0 +1,119 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_WRAPPED_ENCODER_HPP_ +#define TAPROOT_WRAPPED_ENCODER_HPP_ + +#include "tap/communication/sensors/encoder/encoder_interface.hpp" +#include "tap/util_macros.hpp" + +namespace tap::encoder +{ +/** + * Represents an encoder that reports its values as a wrapped number of ticks. + */ +class WrappedEncoder : public EncoderInterface +{ +public: + /** + * @param isInverted if `false` the positive rotation direction of the shaft is + * counter-clockwise when looking at the shaft from. + * If `true` then the positive rotation direction will be clockwise. + * @param encoderResolution the number of encoder ticks before the value wraps. + * @param gearRatio the ratio of input revolutions over output revolutions of this encoder. + * @param encoderHomePosition the zero position for the encoder in encoder ticks. + */ + WrappedEncoder( + bool isInverted, + uint32_t encoderResolution, + float gearRatio = 1, + uint32_t encoderHomePosition = 0); + + void initialize() override{}; + + tap::algorithms::WrappedFloat getPosition() const override; + + /** + * Gets the raw encoder position. + */ + mockable tap::algorithms::WrappedFloat getEncoder() const { return encoder; } + + float getVelocity() const override; + + void alignWith(EncoderInterface* other) override; + + void resetEncoderValue() override; + + DISALLOW_COPY_AND_ASSIGN(WrappedEncoder) + +#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) + bool isOnline() const override { return true; } +#else +protected: +#endif + + /** + * Updates the stored encoder value given a newly received encoder value + * special logic necessary for keeping track of unwrapped encoder value. + */ + void updateEncoderValue(uint32_t encoderActual); + + /** + * The current encoder position. + */ + tap::algorithms::WrappedFloat encoder; + + /** + * The encoder position converted into output rotations + */ + tap::algorithms::WrappedFloat position; + + bool inverted; + + const uint32_t encoderResolution; + + const float gearRatio; + +private: + /** + * The actual encoder wrapped value received from CAN messages where this motor + * is considered to have an encoder value of 0. encoderHomePosition is 0 by default. + */ + tap::algorithms::WrappedFloat encoderHomePosition; + + /** + * The past position for the encoder. Used in velocity calculations. + */ + tap::algorithms::WrappedFloat pastPosition; + + /** + * The last update time for the encoder. Used in velocity calculations. + */ + uint32_t lastUpdateTime; + + uint32_t deltaTime; +}; + +} // namespace tap::encoder + +#endif // TAPROOT_WRAPPED_ENCODER_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp new file mode 100644 index 00000000..732e25a2 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp @@ -0,0 +1,101 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ +#include "abstract_imu.hpp" + +namespace tap::communication::sensors::imu +{ +void AbstractIMU::initialize(float sampleFrequency, float mahonyKp, float mahonyKi) +{ + mahonyAlgorithm.begin(sampleFrequency, mahonyKp, mahonyKi); + imuState = ImuState::IMU_NOT_CALIBRATED; + readTimeout.restart(1'000'000 / sampleFrequency); +} + +void AbstractIMU::requestCalibration() +{ + if (imuState == ImuState::IMU_NOT_CALIBRATED || imuState == ImuState::IMU_CALIBRATED) + { + resetOffsets(); + calibrationSample = 0; + imuState = ImuState::IMU_CALIBRATING; + } +} + +void AbstractIMU::setMountingTransform(const Transform& transform) +{ + mountingTransform = transform.getInverse(); +} + +void AbstractIMU::periodicIMUUpdate() +{ + if (imuState == ImuState::IMU_CALIBRATING) + { + computeOffsets(); + } + else + { + mahonyAlgorithm.updateIMU( + imuData.gyroRadPerSec.x(), + imuData.gyroRadPerSec.y(), + imuData.gyroRadPerSec.z(), + imuData.accG.x(), + imuData.accG.y(), + imuData.accG.z()); + } +} + +void AbstractIMU::resetOffsets() +{ + imuData.accOffsetRaw = {0, 0, 0}; + imuData.gyroOffsetRaw = {0, 0, 0}; +} + +void AbstractIMU::setAccelOffset(float x, float y, float z) +{ + imuData.accOffsetRaw = tap::algorithms::transforms::Vector(x, y, z); +} + +void AbstractIMU::setGyroOffset(float x, float y, float z) +{ + imuData.gyroOffsetRaw = tap::algorithms::transforms::Vector(x, y, z); +} + +void AbstractIMU::computeOffsets() +{ + calibrationSample++; + + imuData.gyroOffsetRaw = imuData.gyroOffsetRaw + imuData.gyroRaw; + imuData.accOffsetRaw = imuData.accOffsetRaw + imuData.accRaw - + tap::algorithms::transforms::Vector(0, 0, getAccelerationSensitivity()); + + if (calibrationSample >= offsetSampleCount) + { + calibrationSample = 0; + imuData.gyroOffsetRaw = imuData.gyroOffsetRaw / offsetSampleCount; + imuData.accOffsetRaw = imuData.accOffsetRaw / offsetSampleCount; + imuState = ImuState::IMU_CALIBRATED; + mahonyAlgorithm.reset(); + } +} + +} // namespace tap::communication::sensors::imu diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp new file mode 100644 index 00000000..b91c6e86 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp @@ -0,0 +1,144 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_ABSTRACT_IMU_HPP_ +#define TAPROOT_ABSTRACT_IMU_HPP_ + +#include "tap/algorithms/MahonyAHRS.h" +#include "tap/algorithms/transforms/orientation.hpp" +#include "tap/algorithms/transforms/transform.hpp" +#include "tap/algorithms/transforms/vector.hpp" +#include "tap/architecture/periodic_timer.hpp" +#include "tap/communication/sensors/imu/imu_interface.hpp" +#include "tap/util_macros.hpp" + +namespace tap::communication::sensors::imu +{ +using tap::algorithms::transforms::Orientation; +using tap::algorithms::transforms::Transform; + +constexpr float GRAVITY_MPS2 = 9.81f; +/** + * A class that represents any directly connected IMU. + */ +class AbstractIMU : public ImuInterface +{ +public: + AbstractIMU(const Transform& mountingTransform = Transform::identity()) + : mountingTransform(mountingTransform.getInverse()) + { + } + + void setMountingTransform(const Transform& transform); + + virtual ~AbstractIMU() = default; + + virtual void initialize(float sampleFrequency, float mahonyKp, float mahonyKi); + + /** + * When this function is called, the bmi088 enters a calibration state during which time, + * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When + * calibrating, angle, accelerometer, and gyroscope values will return 0. When calibrating + * the BMI088 should be level, otherwise the IMU will be calibrated incorrectly. + */ + virtual void requestCalibration() override; + + /** + * Call this function at same rate as intialized sample frequency. + * Performs the mahony AHRS algorithm to compute pitch/roll/yaw. + */ + virtual void periodicIMUUpdate(); + + /** + * Returns the state of the IMU. Can be not connected, connected but not calibrated, or + * calibrated. When not connected, IMU data will be garbage. When not calibrated, IMU data is + * valid but the computed yaw angle data will drift. When calibrating, the IMU data is invalid. + * When calibrated, the IMU data is valid and assuming proper calibration the IMU data should + * not drift. + * + * To be safe, whenever you call the functions below, call this function to ensure + * the data you are about to receive is not garbage. + */ + virtual ImuState getImuState() const { return imuState; } + + mockable inline float getAx() const override { return imuData.accG.x(); } + mockable inline float getAy() const override { return imuData.accG.y(); } + mockable inline float getAz() const override { return imuData.accG.z(); } + mockable inline float getAzMinusG() const { return imuData.accG.z() - GRAVITY_MPS2; } + + mockable inline float getGx() const override { return imuData.gyroRadPerSec.x(); } + mockable inline float getGy() const override { return imuData.gyroRadPerSec.y(); } + mockable inline float getGz() const override { return imuData.gyroRadPerSec.z(); } + + mockable inline float getTemp() const { return imuData.temperature; } + + virtual inline float getYaw() const override { return mahonyAlgorithm.getYaw(); } + virtual inline float getPitch() const override { return mahonyAlgorithm.getPitch(); } + virtual inline float getRoll() const override { return mahonyAlgorithm.getRoll(); } + + struct ImuData + { + tap::algorithms::transforms::Vector accRaw = {0, 0, 0}; + tap::algorithms::transforms::Vector gyroRaw = {0, 0, 0}; + tap::algorithms::transforms::Vector accOffsetRaw = {0, 0, 0}; + tap::algorithms::transforms::Vector gyroOffsetRaw = {0, 0, 0}; + tap::algorithms::transforms::Vector accG = {0, 0, 0}; + tap::algorithms::transforms::Vector gyroRadPerSec = {0, 0, 0}; + + float temperature = 0; + }; + + void setCalibrationSamples(int sampleCount) { offsetSampleCount = sampleCount; } + +protected: + void resetOffsets(); + void computeOffsets(); + void setAccelOffset(float x, float y, float z); + void setGyroOffset(float x, float y, float z); + + inline void applyMountingTransformToRaw(ImuData& data) + { + data.accRaw = mountingTransform.apply(data.accRaw); + data.gyroRaw = mountingTransform.apply(data.gyroRaw); + } + + virtual inline float getAccelerationSensitivity() const = 0; + + tap::algorithms::transforms::Transform mountingTransform; + + Mahony mahonyAlgorithm; + + ImuState imuState = ImuState::IMU_NOT_CONNECTED; + int calibrationSample = 0; + int offsetSampleCount = 1000; + + ImuData imuData; + + tap::arch::PeriodicMicroTimer readTimeout; + + uint32_t prevIMUDataReceivedTime = 0; +}; + +} // namespace tap::communication::sensors::imu + +#endif // TAPROOT_ABSTRACT_IMU_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp index 74f7e1c7..8d95fab8 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp @@ -48,33 +48,9 @@ namespace tap::communication::sensors::imu::bmi088 #define DELAY_US(us) modm::delay_us(us); #endif -Bmi088::Bmi088(tap::Drivers *drivers) : drivers(drivers), imuHeater(drivers) {} - -Bmi088::ImuState Bmi088::getImuState() const { return imuState; } - -void Bmi088::requestRecalibration() -{ - if (imuState == ImuState::IMU_NOT_CALIBRATED || imuState == ImuState::IMU_CALIBRATED) - { - data.gyroOffsetRaw[ImuData::X] = 0; - data.gyroOffsetRaw[ImuData::Y] = 0; - data.gyroOffsetRaw[ImuData::Z] = 0; - data.accOffsetRaw[ImuData::X] = 0; - data.accOffsetRaw[ImuData::Y] = 0; - data.accOffsetRaw[ImuData::Z] = 0; - data.gyroDegPerSec[ImuData::X] = 0; - data.gyroDegPerSec[ImuData::Y] = 0; - data.gyroDegPerSec[ImuData::Z] = 0; - data.accG[ImuData::X] = 0; - data.accG[ImuData::Y] = 0; - data.accG[ImuData::Z] = 0; - calibrationSample = 0; - imuState = ImuState::IMU_CALIBRATING; - } -} - void Bmi088::initialize(float sampleFrequency, float mahonyKp, float mahonyKi) { + AbstractIMU::initialize(sampleFrequency, mahonyKp, mahonyKi); #if !defined(PLATFORM_HOSTED) ImuCS1Accel::GpioOutput(); ImuCS1Gyro::GpioOutput(); @@ -93,8 +69,6 @@ void Bmi088::initialize(float sampleFrequency, float mahonyKp, float mahonyKi) initializeGyro(); imuHeater.initialize(); - - mahonyAlgorithm.begin(sampleFrequency, mahonyKp, mahonyKi); } void Bmi088::initializeAcc() @@ -172,89 +146,44 @@ void Bmi088::initializeGyro() void Bmi088::periodicIMUUpdate() { - if (imuState == ImuState::IMU_NOT_CONNECTED) - { - RAISE_ERROR(drivers, "periodicIMUUpdate called w/ imu not connected"); - return; - } - - if (imuState == ImuState::IMU_CALIBRATING) - { - computeOffsets(); - } - else - { - mahonyAlgorithm.updateIMU( - data.gyroDegPerSec[ImuData::X], - data.gyroDegPerSec[ImuData::Y], - data.gyroDegPerSec[ImuData::Z], - data.accG[ImuData::X], - data.accG[ImuData::Y], - data.accG[ImuData::Z]); - } - - imuHeater.runTemperatureController(data.temperature); + AbstractIMU::periodicIMUUpdate(); + imuHeater.runTemperatureController(imuData.temperature); } -void Bmi088::computeOffsets() +bool Bmi088::read() { - calibrationSample++; - - data.gyroOffsetRaw[ImuData::X] += data.gyroRaw[ImuData::X]; - data.gyroOffsetRaw[ImuData::Y] += data.gyroRaw[ImuData::Y]; - data.gyroOffsetRaw[ImuData::Z] += data.gyroRaw[ImuData::Z]; - data.accOffsetRaw[ImuData::X] += data.accRaw[ImuData::X]; - data.accOffsetRaw[ImuData::Y] += data.accRaw[ImuData::Y]; - data.accOffsetRaw[ImuData::Z] += - data.accRaw[ImuData::Z] - (tap::algorithms::ACCELERATION_GRAVITY / ACC_G_PER_ACC_COUNT); - - if (calibrationSample >= BMI088_OFFSET_SAMPLES) + if (!readTimeout.execute()) { - calibrationSample = 0; - data.gyroOffsetRaw[ImuData::X] /= BMI088_OFFSET_SAMPLES; - data.gyroOffsetRaw[ImuData::Y] /= BMI088_OFFSET_SAMPLES; - data.gyroOffsetRaw[ImuData::Z] /= BMI088_OFFSET_SAMPLES; - data.accOffsetRaw[ImuData::X] /= BMI088_OFFSET_SAMPLES; - data.accOffsetRaw[ImuData::Y] /= BMI088_OFFSET_SAMPLES; - data.accOffsetRaw[ImuData::Z] /= BMI088_OFFSET_SAMPLES; - imuState = ImuState::IMU_CALIBRATED; - mahonyAlgorithm.reset(); + return false; } -} -void Bmi088::read() -{ uint8_t rxBuff[6] = {}; Bmi088Hal::bmi088AccReadMultiReg(Acc::ACC_X_LSB, rxBuff, 6); prevIMUDataReceivedTime = tap::arch::clock::getTimeMicroseconds(); - data.accRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); - data.accRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); - data.accRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); + float rawAccX = bigEndianInt16ToFloat(rxBuff); + float rawAccY = bigEndianInt16ToFloat(rxBuff + 2); + float rawAccZ = bigEndianInt16ToFloat(rxBuff + 4); + imuData.accRaw = tap::algorithms::transforms::Vector(rawAccX, rawAccY, rawAccZ); Bmi088Hal::bmi088GyroReadMultiReg(Gyro::RATE_X_LSB, rxBuff, 6); - data.gyroRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff); - data.gyroRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2); - data.gyroRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4); + + float rawGyroX = bigEndianInt16ToFloat(rxBuff); + float rawGyroY = bigEndianInt16ToFloat(rxBuff + 2); + float rawGyroZ = bigEndianInt16ToFloat(rxBuff + 4); + imuData.gyroRaw = tap::algorithms::transforms::Vector(rawGyroX, rawGyroY, rawGyroZ); + + applyMountingTransformToRaw(imuData); Bmi088Hal::bmi088AccReadMultiReg(Acc::TEMP_MSB, rxBuff, 2); - data.temperature = parseTemp(rxBuff[0], rxBuff[1]); - - data.gyroDegPerSec[ImuData::X] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::X] - data.gyroOffsetRaw[ImuData::X]); - data.gyroDegPerSec[ImuData::Y] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Y] - data.gyroOffsetRaw[ImuData::Y]); - data.gyroDegPerSec[ImuData::Z] = - GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Z] - data.gyroOffsetRaw[ImuData::Z]); - - data.accG[ImuData::X] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::X] - data.accOffsetRaw[ImuData::X]); - data.accG[ImuData::Y] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Y] - data.accOffsetRaw[ImuData::Y]); - data.accG[ImuData::Z] = - ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Z] - data.accOffsetRaw[ImuData::Z]); + imuData.temperature = parseTemp(rxBuff[0], rxBuff[1]); + + imuData.gyroRadPerSec = + (imuData.gyroRaw - imuData.gyroOffsetRaw) * GYRO_RAD_PER_S_PER_GYRO_COUNT; + imuData.accG = (imuData.accRaw - imuData.accOffsetRaw) * ACC_G_PER_ACC_COUNT; + return true; } void Bmi088::setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value) diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp index 1c546658..5e70867f 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp @@ -26,19 +26,17 @@ #include "tap/algorithms/MahonyAHRS.h" #include "tap/algorithms/math_user_utils.hpp" +#include "tap/communication/sensors/imu/abstract_imu.hpp" #include "tap/communication/sensors/imu/imu_interface.hpp" #include "tap/communication/sensors/imu_heater/imu_heater.hpp" #include "tap/util_macros.hpp" -#include "modm/processing/protothread.hpp" - #include "bmi088_data.hpp" namespace tap { class Drivers; } - namespace tap::communication::sensors::imu::bmi088 { /** @@ -56,25 +54,29 @@ namespace tap::communication::sensors::imu::bmi088 * expect * - roll and pitch to be defined as. */ -class Bmi088 final_mockable : public Bmi088Data, public ImuInterface +class Bmi088 final_mockable : public Bmi088Data, public AbstractIMU { public: + Bmi088(Drivers *drivers) : AbstractIMU(), drivers(drivers), imuHeater(drivers) {} + DISALLOW_COPY_AND_ASSIGN(Bmi088) + mockable ~Bmi088() = default; + static constexpr Acc::AccRange_t ACC_RANGE = Acc::AccRange::G3; static constexpr Gyro::GyroRange_t GYRO_RANGE = Gyro::GyroRange::DPS2000; /** - * The maximum angular velocity in degrees / second that the gyro can read based on GYRO_RANGE + * The maximum angular velocity in radians / second that the gyro can read based on GYRO_RANGE * specified above. */ - static constexpr float GYRO_RANGE_MAX_DS = 2000.0f; + static constexpr float GYRO_RANGE_MAX_RAD_PER_S = modm::toRadian(2000.0f); static constexpr float BMI088_TEMP_FACTOR = 0.125f; static constexpr float BMI088_TEMP_OFFSET = 23.0f; /** - * Used to convert raw gyro values to units of degrees / second. Ratio has units - * (degrees / second) / gyro counts. + * Used to convert raw gyro values to units of radians / second. Ratio has units + * (radians / second) / gyro counts. */ - static constexpr float GYRO_DS_PER_GYRO_COUNT = GYRO_RANGE_MAX_DS / 32767.0f; + static constexpr float GYRO_RAD_PER_S_PER_GYRO_COUNT = GYRO_RANGE_MAX_RAD_PER_S / 32767.0f; /** * Refer to page 27 of the bmi088 datasheet for explination of this equation. @@ -83,71 +85,26 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface static constexpr float ACC_G_PER_ACC_COUNT = modm::pow(2, ACC_RANGE.value + 1) * 1.5f * tap::algorithms::ACCELERATION_GRAVITY / 32768.0f; - /** - * The number of samples we take in order to determine the mpu offsets. - */ - float BMI088_OFFSET_SAMPLES = 1000; - - Bmi088(tap::Drivers *drivers); - /** * Starts and configures the bmi088. Blocks for < 200 ms. */ - mockable void initialize(float sampleFrequency, float mahonyKp, float mahonyKi); + mockable void initialize(float sampleFrequency, float mahonyKp, float mahonyKi) override; /** * Call this function at same rate as intialized sample frequency. * Performs the mahony AHRS algorithm to compute pitch/roll/yaw. */ - mockable void periodicIMUUpdate(); + mockable void periodicIMUUpdate() override; /** * This function reads the IMU data from SPI * * @note This function blocks for 129 microseconds to read registers from the BMI088. */ - mockable void read(); - - /** - * Returns the state of the IMU. Can be not connected, connected but not calibrated, or - * calibrated. When not connected, IMU data will be garbage. When not calibrated, IMU data is - * valid but the computed yaw angle data will drift. When calibrating, the IMU data is invalid. - * When calibrated, the IMU data is valid and assuming proper calibration the IMU data should - * not drift. - * - * To be safe, whenever you call the functions below, call this function to ensure - * the data you are about to receive is not garbage. - */ - mockable ImuState getImuState() const; - - /** - * When this function is called, the bmi088 enters a calibration state during which time, - * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When - * calibrating, angle, accelerometer, and gyroscope values will return 0. When calibrating - * the BMI088 should be level, otherwise the IMU will be calibrated incorrectly. - */ - mockable void requestRecalibration(); + mockable bool read(); inline const char *getName() const final_mockable { return "bmi088"; } - mockable inline float getYaw() final_mockable { return mahonyAlgorithm.getYaw(); } - mockable inline float getPitch() final_mockable { return mahonyAlgorithm.getPitch(); } - mockable inline float getRoll() final_mockable { return mahonyAlgorithm.getRoll(); } - - mockable inline float getGx() final_mockable { return data.gyroDegPerSec[ImuData::X]; } - mockable inline float getGy() final_mockable { return data.gyroDegPerSec[ImuData::Y]; } - mockable inline float getGz() final_mockable { return data.gyroDegPerSec[ImuData::Z]; } - - mockable inline float getAx() final_mockable { return data.accG[ImuData::X]; } - mockable inline float getAy() final_mockable { return data.accG[ImuData::Y]; } - mockable inline float getAz() final_mockable { return data.accG[ImuData::Z]; } - - mockable inline float getTemp() final_mockable { return data.temperature; } - - mockable inline uint32_t getPrevIMUDataReceivedTime() const { return prevIMUDataReceivedTime; } - - inline void setOffsetSamples(float samples) { BMI088_OFFSET_SAMPLES = samples; } - inline void setAccOversampling(Acc::AccBandwidth oversampling) { accOversampling = oversampling; @@ -162,41 +119,16 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface } private: + Drivers *drivers; + + inline float getAccelerationSensitivity() const override { return ACC_G_PER_ACC_COUNT; } + static constexpr uint16_t RAW_TEMPERATURE_TO_APPLY_OFFSET = 1023; /// Offset parsed temperature reading by this amount if > RAW_TEMPERATURE_TO_APPLY_OFFSET. static constexpr int16_t RAW_TEMPERATURE_OFFSET = -2048; - struct ImuData - { - enum Axis - { - X = 0, - Y = 1, - Z = 2, - }; - - float accRaw[3] = {}; - float gyroRaw[3] = {}; - float accOffsetRaw[3] = {}; - float gyroOffsetRaw[3] = {}; - float accG[3] = {}; - float gyroDegPerSec[3] = {}; - - float temperature; - } data; - - tap::Drivers *drivers; - - ImuState imuState = ImuState::IMU_NOT_CONNECTED; - - Mahony mahonyAlgorithm; - imu_heater::ImuHeater imuHeater; - int calibrationSample = 0; - - uint32_t prevIMUDataReceivedTime = 0; - Acc::AccBandwidth accOversampling = Acc::AccBandwidth::NORMAL; Acc::AccOutputRate accOutputRate = Acc::AccOutputRate::Hz800; @@ -205,8 +137,6 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface Gyro::GyroBandwidth gyroOutputRate = Gyro::GyroBandwidth::ODR1000_BANDWIDTH116; void initializeGyro(); - void computeOffsets(); - void setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value); void setAndCheckGyroRegister(Gyro::Register reg, Gyro::Registers_t value); diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp index fa5541be..e0f8d942 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp @@ -56,57 +56,59 @@ class ImuInterface * Returns the linear acceleration in the x direction, in * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$. */ - virtual inline float getAx() = 0; + virtual inline float getAx() const = 0; /** * Returns the linear acceleration in the y direction, in * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$. */ - virtual inline float getAy() = 0; + virtual inline float getAy() const = 0; /** * Returns the linear acceleration in the z direction, in * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$. */ - virtual inline float getAz() = 0; + virtual inline float getAz() const = 0; /** * Returns the gyroscope reading (rotational speed) in the x direction, in - * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$. + * \f$\frac{\mbox{radians}}{\mbox{second}}\f$. */ - virtual inline float getGx() = 0; + virtual inline float getGx() const = 0; /** * Returns the gyroscope reading (rotational speed) in the y direction, in - * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$. + * \f$\frac{\mbox{radians}}{\mbox{second}}\f$. */ - virtual inline float getGy() = 0; + virtual inline float getGy() const = 0; /** * Returns the gyroscope reading (rotational speed) in the z direction, in - * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$. + * \f$\frac{\mbox{radians}}{\mbox{second}}\f$. */ - virtual inline float getGz() = 0; + virtual inline float getGz() const = 0; /** - * Returns the temperature of the imu in degrees C. + * Returns yaw angle. in radians. */ - virtual inline float getTemp() = 0; + virtual inline float getYaw() const = 0; /** - * Returns yaw angle. in degrees. + * Returns pitch angle in radians. */ - virtual inline float getYaw() = 0; + virtual inline float getPitch() const = 0; /** - * Returns pitch angle in degrees. + * Returns roll angle in radians. */ - virtual inline float getPitch() = 0; + virtual inline float getRoll() const = 0; /** - * Returns roll angle in degrees. + * When this function is called, the IMU enters a calibration state during which time, + * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When + * calibrating, angle, accelerometer, and gyroscope values will return 0. */ - virtual inline float getRoll() = 0; + virtual void requestCalibration() = 0; }; } // namespace tap::communication::sensors::imu diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp index d6e6fd81..9064905e 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp @@ -29,7 +29,7 @@ #include "modm/architecture/interface/register.hpp" -#include "imu_interface.hpp" +#include "abstract_imu.hpp" namespace tap { @@ -46,7 +46,7 @@ namespace tap::communication::sensors::imu class ImuTerminalSerialHandler : public communication::serial::TerminalSerialCallbackInterface { public: - ImuTerminalSerialHandler(Drivers* drivers, ImuInterface* imu) + ImuTerminalSerialHandler(Drivers* drivers, AbstractIMU* imu) : drivers(drivers), imu(imu), subjectsBeingInspected(0) @@ -75,7 +75,7 @@ class ImuTerminalSerialHandler : public communication::serial::TerminalSerialCal Drivers* drivers; - ImuInterface* imu; + AbstractIMU* imu; /** * Each element in the enum corresponds to a sensor that a user may query. diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp new file mode 100644 index 00000000..59a7b86e --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp @@ -0,0 +1,44 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_ +#define TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_ + +#include "tap/communication/sensors/sensor_interface.hpp" + +namespace tap::communication::sensors::voltage +{ +/** + * Interface for a generic voltage sensor. + */ +class VoltageSensorInterface : public tap::communication::sensors::SensorInterface +{ +public: + /** + * @return The voltage read by the voltage sensor, in millivolts. + */ + virtual float getVoltageMv() const = 0; +}; +} // namespace tap::communication::sensors::voltage + +#endif // TAPROOT_CURRENT_SENSOR_INTERFACE_HPP_ diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp index 3665b021..58efcd7d 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp @@ -77,11 +77,6 @@ void RefSerial::messageReceiveCallback(const ReceivedSerialMessage& completeMess decodeToSiteEventData(completeMessage); break; } - case REF_MESSAGE_TYPE_PROJECTILE_SUPPPLIER_ACTION: - { - decodeToProjectileSupplierAction(completeMessage); - break; - } case REF_MESSAGE_TYPE_WARNING_DATA: { decodeToWarningData(completeMessage); @@ -113,11 +108,6 @@ void RefSerial::messageReceiveCallback(const ReceivedSerialMessage& completeMess decodeToRobotBuffs(completeMessage); break; } - case REF_MESSAGE_TYPE_AERIAL_ENERGY_STATUS: - { - decodeToAerialEnergyStatus(completeMessage); - break; - } case REF_MESSAGE_TYPE_RECEIVE_DAMAGE: { decodeToDamageStatus(completeMessage); @@ -220,7 +210,6 @@ bool RefSerial::decodeToAllRobotHP(const ReceivedSerialMessage& message) convertFromLittleEndian(&robotData.allRobotHp.red.engineer2, message.data + 2); convertFromLittleEndian(&robotData.allRobotHp.red.standard3, message.data + 4); convertFromLittleEndian(&robotData.allRobotHp.red.standard4, message.data + 6); - convertFromLittleEndian(&robotData.allRobotHp.red.standard5, message.data + 8); convertFromLittleEndian(&robotData.allRobotHp.red.sentry7, message.data + 10); convertFromLittleEndian(&robotData.allRobotHp.red.outpost, message.data + 12); convertFromLittleEndian(&robotData.allRobotHp.red.base, message.data + 14); @@ -229,7 +218,6 @@ bool RefSerial::decodeToAllRobotHP(const ReceivedSerialMessage& message) convertFromLittleEndian(&robotData.allRobotHp.blue.engineer2, message.data + 18); convertFromLittleEndian(&robotData.allRobotHp.blue.standard3, message.data + 20); convertFromLittleEndian(&robotData.allRobotHp.blue.standard4, message.data + 22); - convertFromLittleEndian(&robotData.allRobotHp.blue.standard5, message.data + 24); convertFromLittleEndian(&robotData.allRobotHp.blue.sentry7, message.data + 26); convertFromLittleEndian(&robotData.allRobotHp.blue.outpost, message.data + 28); convertFromLittleEndian(&robotData.allRobotHp.blue.base, message.data + 30); @@ -248,9 +236,8 @@ bool RefSerial::decodeToSiteEventData(const ReceivedSerialMessage& message) convertFromLittleEndian(&data, message.data); gameData.eventData.siteData.value = data; - gameData.eventData.virtualShieldRemainingPercent = static_cast((data >> 12) & 0x3F); - gameData.eventData.timeSinceLastDartHit = static_cast((data >> 19) & 0xFF); - gameData.eventData.lastDartHit = static_cast((data >> 28) & 0x03); + gameData.eventData.timeSinceLastDartHit = static_cast((data >> 9) & 0xFF); + gameData.eventData.lastDartHit = static_cast((data >> 18) & 0x07); return true; } @@ -325,9 +312,6 @@ bool RefSerial::decodeToPowerAndHeat(const ReceivedSerialMessage& message) { return false; } - convertFromLittleEndian(&robotData.chassis.volt, message.data); - convertFromLittleEndian(&robotData.chassis.current, message.data + 2); - convertFromLittleEndian(&robotData.chassis.power, message.data + 4); convertFromLittleEndian(&robotData.chassis.powerBuffer, message.data + 8); convertFromLittleEndian(&robotData.turret.heat17ID1, message.data + 10); convertFromLittleEndian(&robotData.turret.heat17ID2, message.data + 12); @@ -349,7 +333,7 @@ bool RefSerial::decodeToRobotPosition(const ReceivedSerialMessage& message) bool RefSerial::decodeToRobotBuffs(const ReceivedSerialMessage& message) { - if (message.header.dataLength != 6) + if (message.header.dataLength != 7) { return false; } @@ -359,6 +343,8 @@ bool RefSerial::decodeToRobotBuffs(const ReceivedSerialMessage& message) robotData.robotBuffStatus.vulnerabilityBuff = message.data[3]; convertFromLittleEndian(&robotData.robotBuffStatus.attackBuff, message.data + 4); + robotData.robotEnergyRemaining = static_cast(message.data[6]); + return true; } @@ -446,8 +432,6 @@ bool RefSerial::decodeToGroundPositions(const ReceivedSerialMessage& message) convertFromLittleEndian(&gameData.positions.standard3.y, message.data + 20); convertFromLittleEndian(&gameData.positions.standard4.x, message.data + 24); convertFromLittleEndian(&gameData.positions.standard4.y, message.data + 28); - convertFromLittleEndian(&gameData.positions.standard5.x, message.data + 32); - convertFromLittleEndian(&gameData.positions.standard5.y, message.data + 36); return true; } @@ -462,7 +446,6 @@ bool RefSerial::decodeToRadarProgress(const ReceivedSerialMessage& message) gameData.radarProgress.engineer = message.data[2]; gameData.radarProgress.standard3 = message.data[3]; gameData.radarProgress.standard4 = message.data[4]; - gameData.radarProgress.standard5 = message.data[5]; gameData.radarProgress.sentry = message.data[6]; return true; diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp index 988534d1..d35cad73 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp @@ -81,8 +81,8 @@ class RefSerial : public DJISerial, public RefSerialData * parser yet. They are values that are used in message headers to indicate the type of message * we have received. * - * Current Ref Serial Version: 1.6.1 - * Updated April 2024. + * Current Ref Serial Version: 1.7.0 + * Updated March 2025. */ enum MessageType { @@ -91,7 +91,6 @@ class RefSerial : public DJISerial, public RefSerialData REF_MESSAGE_TYPE_ALL_ROBOT_HP = 0x3, REF_MESSAGE_TYPE_SITE_EVENT_DATA = 0x101, - REF_MESSAGE_TYPE_PROJECTILE_SUPPPLIER_ACTION = 0x102, REF_MESSAGE_TYPE_WARNING_DATA = 0x104, REF_MESSAGE_TYPE_DART_INFO = 0x105, @@ -99,7 +98,6 @@ class RefSerial : public DJISerial, public RefSerialData REF_MESSAGE_TYPE_POWER_AND_HEAT = 0x202, REF_MESSAGE_TYPE_ROBOT_POSITION = 0x203, REF_MESSAGE_TYPE_ROBOT_BUFF_STATUS = 0x204, - REF_MESSAGE_TYPE_AERIAL_ENERGY_STATUS = 0x205, REF_MESSAGE_TYPE_RECEIVE_DAMAGE = 0x206, REF_MESSAGE_TYPE_PROJECTILE_LAUNCH = 0x207, REF_MESSAGE_TYPE_BULLETS_REMAIN = 0x208, @@ -111,9 +109,14 @@ class RefSerial : public DJISerial, public RefSerialData REF_MESSAGE_TYPE_RADAR_INFO = 0x20E, REF_MESSAGE_TYPE_CUSTOM_DATA = 0x301, - // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER = 0x302, - // REF_MESSAGE_TYPE_SMALL_MAP = 0x303, - // REF_MESSAGE_TYPE_VTM_INPUT_DATA = 0x304 + // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER_DATA_RECEIVE = 0x302, + // REF_MESSAGE_TYPE_SMALL_MAP_INTERACTION = 0x303, + // REF_MESSAGE_TYPE_VTM_INPUT_DATA = 0x304, + // REF_MESSAGE_TYPE_RADAR_MINIMAP = 0x305, + // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER_DATA_SEND = 0x306, + // REF_MESSAGE_TYPE_SENTRY_SMALL_MAP = 0x307, + // REF_MESSAGE_TYPE_ROBOT_SMALL_MAP = 0x308, + // REF_MESSAGE_TYPE_ROBOT_CUSTOM_CONTROLLER_FEEDBACK = 0x309 }; /** @@ -169,11 +172,11 @@ class RefSerial : public DJISerial, public RefSerialData return false; } - mockable void releaseTransmissionSemaphore(uint32_t sentMsgLen) + mockable void releaseTransmissionSemaphore() { transmissionSemaphore.release(); transmissionDelayTimer.restart( - std::ceil(sentMsgLen * 1000.0f / Tx::MAX_TRANSMIT_SPEED_BYTES_PER_S)); + std::ceil(1.0f / RefSerialData::Tx::ROBOT_INTERACTION_DATA_RATE * 1000.0f)); } /** diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp index 2d8c401a..486b6ad3 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp @@ -117,10 +117,11 @@ class RefSerialData enum class SiteDartHit : uint8_t { - NONE = 0, ///< No hit target. - OUTPOST = 1, ///< Outpost hit. - BASE_FIXED = 2, ///< Fixed target hit. - BASE_RANDOM = 3 ///< Random target hit. + NONE = 0, ///< No hit target. + OUTPOST = 1, ///< Outpost hit. + BASE_FIXED = 2, ///< Fixed target hit. + BASE_RANDOM_FIXED = 3, ///< Fixed target hit after random movement. + BASE_RANDOM_MOVING = 4 ///< Random moving target hit. }; enum class SupplierOutletStatus : uint8_t @@ -158,25 +159,20 @@ class RefSerialData enum class SiteData : uint32_t { - RESTORATION_FRONT_OCCUPIED = modm::Bit0, - RESTORATION_INSIDE_OCCUPIED = modm::Bit1, + RESUPPLY_OUTSIDE_EXCHANGE_OCCUPIED = modm::Bit0, + RESUPPLY_INSIDE_EXCHANGE_OCCUPIED = modm::Bit1, SUPPLIER_OCCUPIED = modm::Bit2, - POWER_RUNE_OCCUPIED = modm::Bit3, - SMALL_POWER_RUNE_ACTIVATED = modm::Bit4, - LARGER_POWER_RUNE_ACTIVIATED = modm::Bit5, + SMALL_POWER_RUNE_ACTIVATED = modm::Bit3, + LARGER_POWER_RUNE_ACTIVIATED = modm::Bit4, - RING_OCCUPIED_TEAM = modm::Bit6, - RING_OCCUPIED_OPPONENT = modm::Bit7, + CENTRAL_ELEVATED_GROUND_OCCUPIED_TEAM = modm::Bit5, + CENTRAL_ELEVATED_GROUND_OCCUPIED_OPPONENT = modm::Bit6, - TRAPEZOID_R3_OCCUPIED_TEAM = modm::Bit8, - TRAPEZOID_R3_OCCUPIED_OPPONENT = modm::Bit9, + TRAPEZOID_OCCUPIED_TEAM = modm::Bit7, - TRAPEZOID_R4_OCCUPIED_TEAM = modm::Bit10, - TRAPEZOID_R4_OCCUPIED_OPPONENT = modm::Bit11, - - CENTRAL_BUFF_OCCUPIED_TEAM = modm::Bit30, - CENTRAL_BUFF_OCCUPIED_OPPONENT = modm::Bit31 + CENTRAL_BUFF_OCCUPIED_TEAM = modm::Bit21, + CENTRAL_BUFF_OCCUPIED_OPPONENT = modm::Bit22 }; MODM_FLAGS32(SiteData); @@ -188,29 +184,43 @@ class RefSerialData }; MODM_FLAGS8(RobotPower); + enum class RobotEnergyLevel : uint8_t + { + ABOVE_50_PERCENT = 0x32, + ABOVE_30_PERCENT = 0b11110, + ABOVE_15_PERCENT = 0b11100, + ABOVE_5_PERCENT = 0b11000, + ABOVE_1_PERCENT = 0b10000, + BELOW_1_PERCENT = 0b00000, + }; + /// Activation status flags for the RFID module (for RMUC only). enum class RFIDActivationStatus : uint32_t { BASE_BUFF = modm::Bit0, - ELEVATED_RING_OWN = modm::Bit1, - ELEVATED_RING_OPPONENT = modm::Bit2, - TRAPEZOID_R3_OWN = modm::Bit3, - TRAPEZOID_R3_OPPONENT = modm::Bit4, - TRAPEZOID_R4_OWN = modm::Bit5, - TRAPEZOID_R4_OPPONENT = modm::Bit6, - POWER_RUNE_ACTIVATION = modm::Bit7, - LAUNCH_RAMP_FRONT_OWN = modm::Bit8, - LAUNCH_RAMP_BACK_OWN = modm::Bit9, - LAUNCH_RAMP_FRONT_OPPONENT = modm::Bit10, - LAUNCH_RAMP_BACK_OPPONENT = modm::Bit11, - OUTPOST_BUFF = modm::Bit12, - RESTORATION_ZONE = modm::Bit13, - SENTRY_PATROL_OWN = modm::Bit14, - SENTRY_PATROL_OPPONENT = modm::Bit15, - LARGE_ISLAND_OWN = modm::Bit16, - LARGE_ISLAND_OPPONENT = modm::Bit17, - EXCHANGE_ZONE = modm::Bit18, - CENTRAL_BUFF = modm::Bit19 + CENTRAL_ELEVATED_GROUND_OWN = modm::Bit1, + CENTRAL_ELEVATED_GROUND_OPPONENT = modm::Bit2, + TRAPEZOID_OWN = modm::Bit3, + TRAPEZOID_OPPONENT = modm::Bit4, + LAUNCH_RAMP_FRONT_OWN = modm::Bit5, + LAUNCH_RAMP_BACK_OWN = modm::Bit6, + LAUNCH_RAMP_FRONT_OPPONENT = modm::Bit7, + LAUNCH_RAMP_BACK_OPPONENT = modm::Bit8, + TERRAIN_CROSSING_OFF_CENTRAL_OWN = modm::Bit9, + TERRAIN_CROSSING_ON_CENTRAL_OWN = modm::Bit10, + TERRAIN_CROSSING_OFF_CENTRAL_OPPONENT = modm::Bit11, + TERRAIN_CROSSING_ON_CENTRAL_OPPONENT = modm::Bit12, + TERRAIN_CROSSING_OFF_ROAD_OWN = modm::Bit13, + TERRAIN_CROSSING_ON_ROAD_OWN = modm::Bit14, + TERRAIN_CROSSING_OFF_ROAD_OPPONENT = modm::Bit15, + TERRAIN_CROSSING_ON_ROAD_OPPONENT = modm::Bit16, + FORTRESS_BUFF_OWN = modm::Bit17, + OUTPOST_BUFF_OWN = modm::Bit18, + RESUPPLY_ZONE_OUTSIDE_EXCHANGE = modm::Bit19, + RESUPPLY_ZONE_INSIDE_EXCHANGE = modm::Bit20, + LARGE_RESOURCE_ISLAND_OWN = modm::Bit21, + LARGE_RESOURCE_ISLAND_OPPONENT = modm::Bit22, + CENTRAL_BUFF = modm::Bit23 }; MODM_FLAGS32(RFIDActivationStatus); @@ -240,7 +250,7 @@ class RefSerialData /** * The Maximum launch speed for a 17mm barrel in m/s. */ - static constexpr int MAX_LAUNCH_SPEED_17MM = 30; + static constexpr int MAX_LAUNCH_SPEED_17MM = 25; /** * The Maximum launch speed for a 42mm barrel in m/s. @@ -258,7 +268,6 @@ class RefSerialData uint16_t engineer2; uint16_t standard3; uint16_t standard4; - uint16_t standard5; uint16_t sentry7; uint16_t outpost; uint16_t base; @@ -273,9 +282,7 @@ class RefSerialData */ struct EventData { - SiteData_t siteData; ///< Information about occupied zones. - uint8_t - virtualShieldRemainingPercent; ///< Remain percent on own base's virtual shield. + SiteData_t siteData; ///< Information about occupied zones. uint8_t timeSinceLastDartHit; ///< Time since the last dart hit own outpost or base. SiteDartHit lastDartHit; ///< The target hit by the last dart. }; @@ -301,9 +308,6 @@ class RefSerialData struct ChassisData { - uint16_t volt; ///< Output voltage to the chassis (in mV). - uint16_t current; ///< Output current to the chassis (in mA). - float power; ///< Output power to the chassis (in W). uint16_t powerBuffer; ///< Chassis power buffer (in J). RobotPosition position; ///< x, y coordinate of the chassis (in m). uint16_t powerConsumptionLimit; ///< The current chassis power limit (in W). @@ -415,7 +419,6 @@ class RefSerialData RobotPosition engineer; RobotPosition standard3; RobotPosition standard4; - RobotPosition standard5; }; /** @@ -427,7 +430,6 @@ class RefSerialData uint8_t engineer; uint8_t standard3; uint8_t standard4; - uint8_t standard5; uint8_t sentry; }; @@ -490,6 +492,7 @@ class RefSerialData ///< received. RefereeWarningData refereeWarningData; ///< Referee warning information, updated when ///< a robot receives a penalty + RobotEnergyLevel robotEnergyRemaining; ///< The current energy level of the robot. }; }; @@ -652,13 +655,10 @@ class RefSerialData } modm_packed; /** - * You cannot send messages faster than this speed to the referee system. - * - * Source: https://bbs.robomaster.com/forum.php?mod=viewthread&tid=9120 - * - * Changed from 1280 to 1000 as the HUD was still unreliable. + * As defined as the rate (in hz) at which the 0x0301 message is sent to the referee + * system. */ - static constexpr uint32_t MAX_TRANSMIT_SPEED_BYTES_PER_S = 1000; + static constexpr uint8_t ROBOT_INTERACTION_DATA_RATE = 30; /** * Get the min wait time after which you can send more data to the client. Sending faster @@ -686,7 +686,7 @@ class RefSerialData "Invalid type, getWaitTimeAfterGraphicSendMs only takes in ref serial message " "types."); - return sizeof(T) * 1'000 / MAX_TRANSMIT_SPEED_BYTES_PER_S; + return 1'000 / ROBOT_INTERACTION_DATA_RATE; } }; }; diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp index 22272145..9a474928 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp @@ -234,7 +234,7 @@ modm::ResumableResult RefSerialTransmitter::deleteGraphicLayer( reinterpret_cast(&deleteGraphicLayerMessage), sizeof(Tx::DeleteGraphicLayerMessage)); - drivers->refSerial.releaseTransmissionSemaphore(sizeof(Tx::DeleteGraphicLayerMessage)); + drivers->refSerial.releaseTransmissionSemaphore(); RF_END(); } @@ -285,7 +285,7 @@ modm::ResumableResult RefSerialTransmitter::sendGraphic_( reinterpret_cast(graphicMsg), sizeof(*graphicMsg)); - drivers->refSerial.releaseTransmissionSemaphore(sizeof(GRAPHIC)); + drivers->refSerial.releaseTransmissionSemaphore(); } RF_END(); } @@ -433,8 +433,7 @@ modm::ResumableResult RefSerialTransmitter::sendRobotToRobotMsg( reinterpret_cast(robotToRobotMsg), FULL_MSG_SIZE_LESS_MSGLEN + msgLen + sizeof(uint16_t)); - drivers->refSerial.releaseTransmissionSemaphore( - FULL_MSG_SIZE_LESS_MSGLEN + msgLen + sizeof(uint16_t)); + drivers->refSerial.releaseTransmissionSemaphore(); RF_END(); } diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp index abefc6ce..aeb4d635 100644 --- a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp +++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp @@ -54,9 +54,11 @@ string makeMotorMessage(const tap::motor::DjiMotor& motor) "\"canBus\":" + std::to_string(static_cast(motor.getCanBus()) + 1) + "," + "\"motorID\":" + std::to_string(motor.getMotorIdentifier()) + "," + - "\"shaftRPM\":" + std::to_string(motor.getShaftRPM()) + "," + - "\"torque\":" + std::to_string(motor.getTorque()) + "," + - "\"encoderValue\":" + std::to_string(motor.getEncoderUnwrapped()) + "}"; + "\"shaftRPM\":" + std::to_string(motor.getInternalEncoder().getShaftRPM()) + "," + + "\"torque\":" + std::to_string(motor.getTorque()) + "," + "\"encoderValue\":" + + std::to_string( + static_cast(motor.getInternalEncoder().getEncoder().getWrappedValue())) + + "}"; return jsonMessage; } diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp index d180a095..e32b5895 100644 --- a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp +++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp @@ -28,10 +28,12 @@ #include -#include "tap/motor/dji_motor.hpp" - namespace tap { +namespace motor +{ +class DjiMotor; +} namespace communication { /** diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp index c8903c48..726525ef 100644 --- a/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp +++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp @@ -39,6 +39,8 @@ #include #include +#include "tap/util_macros.hpp" + #include "json_messages.hpp" using std::cerr; /** diff --git a/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp b/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp index 14657052..15987f95 100644 --- a/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp +++ b/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp @@ -25,7 +25,6 @@ #define TAPROOT_CHASSIS_SUBSYSTEM_INTERFACE_HPP_ #include "tap/algorithms/math_user_utils.hpp" -#include "tap/motor/dji_motor.hpp" #include "../subsystem.hpp" #include "modm/math/matrix.hpp" diff --git a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp index 203b6c82..d31272d7 100644 --- a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp +++ b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp @@ -35,11 +35,13 @@ namespace tap::control::chassis PowerLimiter::PowerLimiter( const tap::Drivers *drivers, tap::communication::sensors::current::CurrentSensorInterface *currentSensor, + tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor, float startingEnergyBuffer, float energyBufferLimitThreshold, float energyBufferCritThreshold) : drivers(drivers), currentSensor(currentSensor), + voltageSensor(voltageSensor), startingEnergyBuffer(startingEnergyBuffer), energyBufferLimitThreshold(energyBufferLimitThreshold), energyBufferCritThreshold(energyBufferCritThreshold), @@ -79,8 +81,9 @@ void PowerLimiter::updatePowerAndEnergyBuffer() { const auto &robotData = drivers->refSerial.getRobotData(); const auto &chassisData = robotData.chassis; - const float current = currentSensor->getCurrentMa(); - const float newChassisPower = chassisData.volt * current / 1'000'000.0f; + const float current = currentSensor->getCurrentMa() / 1000.0f; + const float voltage = voltageSensor->getVoltageMv() / 1000.0f; + const float newChassisPower = voltage * current; // Manually compute energy buffer using consumedPower read from current sensor. // See rules manual for reasoning behind the energy buffer calculation. diff --git a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp index 4966dcaf..f3cd6a0f 100644 --- a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp +++ b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp @@ -26,6 +26,7 @@ #include "tap/communication/gpio/analog.hpp" #include "tap/communication/sensors/current/current_sensor_interface.hpp" +#include "tap/communication/sensors/voltage/voltage_sensor_interface.hpp" namespace tap { @@ -80,6 +81,7 @@ class PowerLimiter PowerLimiter( const tap::Drivers *drivers, tap::communication::sensors::current::CurrentSensorInterface *currentSensor, + tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor, float startingEnergyBuffer, float energyBufferLimitThreshold, float energyBufferCritThreshold); @@ -102,6 +104,7 @@ class PowerLimiter private: const tap::Drivers *drivers; tap::communication::sensors::current::CurrentSensorInterface *currentSensor; + tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor; const float startingEnergyBuffer; const float energyBufferLimitThreshold; const float energyBufferCritThreshold; diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp index fba45d7d..f7b00889 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp @@ -25,7 +25,6 @@ #define TAPROOT_CALIBRATE_COMMAND_HPP_ #include "tap/control/command.hpp" -#include "tap/drivers.hpp" namespace tap { diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp index 97af1c09..924a749a 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp @@ -29,7 +29,6 @@ #include "tap/architecture/timeout.hpp" #include "tap/control/command.hpp" #include "tap/control/setpoint/interfaces/setpoint_subsystem.hpp" -#include "tap/drivers.hpp" #include "modm/math/filter/pid.hpp" diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp index e3a6d973..9071f9a0 100644 --- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp +++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp @@ -29,8 +29,6 @@ #include "tap/algorithms/ramp.hpp" #include "tap/architecture/timeout.hpp" #include "tap/control/command.hpp" -#include "tap/drivers.hpp" -#include "tap/motor/dji_motor.hpp" namespace tap { diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp index cb424896..5b1adb59 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp @@ -47,21 +47,26 @@ DjiMotor::DjiMotor( tap::can::CanBus motorCanBus, bool isInverted, const char* name, - uint16_t encoderWrapped, - int64_t encoderRevolutions) + bool currentControl, + float gearRatio, + uint32_t encoderHomePosition, + tap::encoder::EncoderInterface* externalEncoder) : CanRxListener(drivers, static_cast(desMotorIdentifier), motorCanBus), motorName(name), drivers(drivers), motorIdentifier(desMotorIdentifier), motorCanBus(motorCanBus), desiredOutput(0), - shaftRPM(0), temperature(0), torque(0), motorInverted(isInverted), - encoderWrapped(encoderWrapped), - encoderRevolutions(encoderRevolutions), - encoderHomePosition(0) + currentControl(currentControl), + internalEncoder(isInverted, gearRatio, encoderHomePosition), + encoder( + {externalEncoder != nullptr ? externalEncoder + : const_cast(&this->getInternalEncoder()), + externalEncoder != nullptr ? const_cast(&this->getInternalEncoder()) + : nullptr}) { motorDisconnectTimeout.stop(); } @@ -70,6 +75,7 @@ void DjiMotor::initialize() { drivers->djiMotorTxHandler.addMotorToManager(this); attachSelfToRxHandler(); + this->encoder.initialize(); } void DjiMotor::processMessage(const modm::can::Message& message) @@ -78,10 +84,6 @@ void DjiMotor::processMessage(const modm::can::Message& message) { return; } - uint16_t encoderActual = - static_cast(message.data[0] << 8 | message.data[1]); // encoder value - shaftRPM = static_cast(message.data[2] << 8 | message.data[3]); // rpm - shaftRPM = motorInverted ? -shaftRPM : shaftRPM; torque = static_cast(message.data[4] << 8 | message.data[5]); // torque torque = motorInverted ? -torque : torque; temperature = static_cast(message.data[6]); // temperature @@ -89,14 +91,7 @@ void DjiMotor::processMessage(const modm::can::Message& message) // restart disconnect timer, since you just received a message from the motor motorDisconnectTimeout.restart(MOTOR_DISCONNECT_TIME); - // invert motor if necessary - encoderActual = motorInverted ? ENC_RESOLUTION - 1 - encoderActual : encoderActual; - - int32_t encoderRelativeToHome = (int32_t)encoderActual - (int32_t)encoderHomePosition; - - updateEncoderValue( - encoderRelativeToHome < 0 ? (int32_t)ENC_RESOLUTION + encoderRelativeToHome - : encoderRelativeToHome); + this->internalEncoder.processMessage(message); } void DjiMotor::setDesiredOutput(int32_t desiredOutput) @@ -136,52 +131,14 @@ int8_t DjiMotor::getTemperature() const { return temperature; } int16_t DjiMotor::getTorque() const { return torque; } -int16_t DjiMotor::getShaftRPM() const { return shaftRPM; } - bool DjiMotor::isMotorInverted() const { return motorInverted; } tap::can::CanBus DjiMotor::getCanBus() const { return motorCanBus; } const char* DjiMotor::getName() const { return motorName; } -int64_t DjiMotor::getEncoderUnwrapped() const -{ - return static_cast(encoderWrapped) + - static_cast(ENC_RESOLUTION) * encoderRevolutions; -} - -uint16_t DjiMotor::getEncoderWrapped() const { return encoderWrapped; } +bool DjiMotor::isInCurrentControl() const { return currentControl; } -void DjiMotor::resetEncoderValue() -{ - encoderRevolutions = 0; - encoderHomePosition = (encoderWrapped + encoderHomePosition) % ENC_RESOLUTION; - encoderWrapped = 0; -} - -float DjiMotor::getPositionUnwrapped() const -{ - return getEncoderUnwrapped() * M_TWOPI / ENC_RESOLUTION; -} - -float DjiMotor::getPositionWrapped() const -{ - return getEncoderWrapped() * M_TWOPI / ENC_RESOLUTION; -} - -void DjiMotor::updateEncoderValue(uint16_t newEncWrapped) -{ - int16_t enc_dif = newEncWrapped - encoderWrapped; - if (enc_dif < -ENC_RESOLUTION / 2) - { - encoderRevolutions++; - } - else if (enc_dif > ENC_RESOLUTION / 2) - { - encoderRevolutions--; - } - encoderWrapped = newEncWrapped; -} } // namespace motor } // namespace tap diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp index 3b970524..6d35324c 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp @@ -28,30 +28,27 @@ #include "tap/architecture/timeout.hpp" #include "tap/communication/can/can_rx_listener.hpp" +#include "tap/communication/sensors/encoder/multi_encoder.hpp" #include "modm/math/geometry/angle.hpp" +#include "dji_motor_encoder.hpp" +#include "dji_motor_ids.hpp" #include "motor_interface.hpp" +#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) +#include + +#include "tap/mock/dji_motor_encoder_mock.hpp" +#endif + namespace tap::motor { -/** - * CAN IDs for the feedback messages sent by DJI motor controllers. Motor `i` in the set - * {1, 2,...,8} sends feedback data with in a CAN message with ID 0x200 + `i`. - * for declaring a new motor, must be one of these motor - * identifiers - */ -enum MotorId : uint32_t -{ - MOTOR1 = 0X201, - MOTOR2 = 0x202, - MOTOR3 = 0x203, - MOTOR4 = 0x204, - MOTOR5 = 0x205, - MOTOR6 = 0x206, - MOTOR7 = 0x207, - MOTOR8 = 0x208, -}; +#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) +using Encoder = tap::mock::DjiMotorEncoderMock; +#else +using Encoder = DjiMotorEncoder; +#endif /** * A class designed to interface with DJI brand motors and motor controllers over CAN. @@ -62,11 +59,7 @@ enum MotorId : uint32_t * == false`) is counter clockwise when looking at the shaft from the side opposite * the motor. This is specified in the C620 user manual (page 18). * - * DJI motor encoders store a consistent encoding for a given angle across power-cycles. - * This means the encoder angle reported by the motor can have meaning if the encoding - * for an angle is unique as it is for the GM6020s. However for geared motors like the - * M3508 where a full encoder revolution does not correspond 1:1 to a shaft revolution, - * it is impossible to know the orientation of the shaft given just the encoder value. + * @see DjiMotorEncoder * * Extends the CanRxListener class to attach a message handler for feedback data from the * motor to the CAN Rx dispatch handler. @@ -77,9 +70,6 @@ enum MotorId : uint32_t class DjiMotor : public can::CanRxListener, public MotorInterface { public: - // 0 - 8191 for dji motors - static constexpr uint16_t ENC_RESOLUTION = 8192; - // Maximum values for following motors // Controller for the M2006, in mA output static constexpr uint16_t MAX_OUTPUT_C610 = 10000; @@ -93,14 +83,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface // Output is in mV static constexpr uint16_t MAX_OUTPUT_GM3510 = 29000; - // Internal gear ratio of the following motors - static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f; - static constexpr float GEAR_RATIO_M3510_L1 = 3.7f / 1.0f; - static constexpr float GEAR_RATIO_M3510_L2 = 5.2f / 1.0f; - static constexpr float GEAR_RATIO_M3510_L3 = 19.0f / 1.0f; - static constexpr float GEAR_RATIO_M3510_L4 = 27.0f / 1.0f; - static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f; - /** * @param drivers a pointer to the drivers struct * @param desMotorIdentifier the ID of this motor controller @@ -109,10 +91,9 @@ class DjiMotor : public can::CanRxListener, public MotorInterface * counter-clockwise when looking at the shaft from the side opposite the motor. * If `true` then the positive rotation direction will be clockwise. * @param name a name to associate with the motor for use in the motor menu - * @param encoderWrapped the starting encoderValue to store for this motor. - * Will be overwritten by the first reported encoder value from the motor - * @param encoderRevolutions the starting number of encoder revolutions to store. - * See comment for DjiMotor::encoderRevolutions for more details. + * @param gearRatio the ratio of input revolutions to output revolutions of this encoder. + * @param encoderHomePosition the zero position for the encoder in encoder ticks. + * @param externalEncoder a pointer to an external encoder to average with the internal encoder. */ DjiMotor( Drivers* drivers, @@ -120,26 +101,24 @@ class DjiMotor : public can::CanRxListener, public MotorInterface tap::can::CanBus motorCanBus, bool isInverted, const char* name, - uint16_t encoderWrapped = ENC_RESOLUTION / 2, - int64_t encoderRevolutions = 0); + bool currentControl = false, + float gearRatio = 1, + uint32_t encoderHomePosition = 0, + tap::encoder::EncoderInterface* externalEncoder = nullptr); mockable ~DjiMotor(); void initialize() override; - float getPositionUnwrapped() const override; - - float getPositionWrapped() const override; - - int64_t getEncoderUnwrapped() const override; - - uint16_t getEncoderWrapped() const override; + tap::encoder::EncoderInterface* getEncoder() const override + { + return const_cast*>(&this->encoder); + } /** - * Resets this motor's current encoder home position to the current encoder position reported by - * CAN messages, and resets this motor's encoder revolutions to 0. + * Returns the builtin encoder associated with the motor. */ - void resetEncoderValue() override; + mockable const Encoder& getInternalEncoder() const { return this->internalEncoder; } DISALLOW_COPY_AND_ASSIGN(DjiMotor) @@ -190,37 +169,13 @@ class DjiMotor : public can::CanRxListener, public MotorInterface int16_t getTorque() const override; - /// For interpreting the sign of return value see class comment - int16_t getShaftRPM() const override; - mockable bool isMotorInverted() const; mockable tap::can::CanBus getCanBus() const; mockable const char* getName() const; - template - static void assertEncoderType() - { - constexpr bool good_type = - std::is_same::type, std::int64_t>::value || - std::is_same::type, std::uint16_t>::value; - static_assert(good_type, "x is not of the correct type"); - } - - template - static T degreesToEncoder(float angle) - { - assertEncoderType(); - return static_cast((ENC_RESOLUTION * angle) / 360); - } - - template - static float encoderToDegrees(T encoder) - { - assertEncoderType(); - return (360.0f * static_cast(encoder)) / ENC_RESOLUTION; - } + mockable bool isInCurrentControl() const; private: // wait time before the motor is considered disconnected, in milliseconds @@ -228,12 +183,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface const char* motorName; - /** - * Updates the stored encoder value given a newly received encoder value - * special logic necessary for keeping track of unwrapped encoder value. - */ - void updateEncoderValue(uint16_t newEncWrapped); - Drivers* drivers; uint32_t motorIdentifier; @@ -242,8 +191,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface int16_t desiredOutput; - int16_t shaftRPM; - int8_t temperature; int16_t torque; @@ -255,26 +202,15 @@ class DjiMotor : public can::CanRxListener, public MotorInterface */ bool motorInverted; - /** - * The raw encoder value reported by the motor controller relative to - * encoderHomePosition. It wraps around from {0..8191}, hence "Wrapped" - */ - uint16_t encoderWrapped; + bool currentControl; - /** - * Absolute unwrapped encoder position = - * encoderRevolutions * ENCODER_RESOLUTION + encoderWrapped - * This lets us keep track of some sense of absolute position even while - * raw encoderValue continuosly loops within {0..8191}. Origin value is - * arbitrary. - */ - int64_t encoderRevolutions; +#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) + testing::NiceMock internalEncoder; +#else + Encoder internalEncoder; +#endif - /** - * The actual encoder wrapped value received from CAN messages where this motor - * is considered to have an encoder value of 0. encoderHomePosition is 0 by default. - */ - uint16_t encoderHomePosition; + tap::encoder::MultiEncoder<2> encoder; tap::arch::MilliTimeout motorDisconnectTimeout; }; diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp new file mode 100644 index 00000000..40243930 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp @@ -0,0 +1,69 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "dji_motor_encoder.hpp" + +#include "tap/algorithms/math_user_utils.hpp" + +namespace tap +{ +namespace motor +{ +DjiMotorEncoder::DjiMotorEncoder(bool isInverted, float gearRatio, uint32_t encoderHomePosition) + : WrappedEncoder(isInverted, ENC_RESOLUTION, gearRatio, encoderHomePosition), + shaftRPM(0) +{ + encoderDisconnectTimeout.stop(); +} + +void DjiMotorEncoder::processMessage(const modm::can::Message& message) +{ + encoderDisconnectTimeout.restart(MOTOR_DISCONNECT_TIME); + shaftRPM = static_cast(message.data[2] << 8 | message.data[3]); // rpm + shaftRPM = inverted ? -shaftRPM : shaftRPM; + + uint16_t encoderActual = + static_cast(message.data[0] << 8 | message.data[1]); // encoder value + + updateEncoderValue(encoderActual); +} + +bool DjiMotorEncoder::isOnline() const +{ + /* + * motor online if the disconnect timout has not expired (if it received message but + * somehow got disconnected) and the timeout hasn't been stopped (initially, the timeout + * is stopped) + */ + return !encoderDisconnectTimeout.isExpired() && !encoderDisconnectTimeout.isStopped(); +} + +float DjiMotorEncoder::getVelocity() const +{ + return this->getShaftRPM() * static_cast(M_TWOPI) / 60.f * this->gearRatio; +} + +int16_t DjiMotorEncoder::getShaftRPM() const { return shaftRPM; } +} // namespace motor + +} // namespace tap diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp new file mode 100644 index 00000000..f6e4b9f7 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp @@ -0,0 +1,112 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_DJI_MOTOR_ENCODER_HPP_ +#define TAPROOT_DJI_MOTOR_ENCODER_HPP_ + +#include "tap/architecture/timeout.hpp" +#include "tap/communication/sensors/encoder/wrapped_encoder.hpp" +#include "tap/util_macros.hpp" + +#include "modm/architecture/interface/can_message.hpp" +#include "modm/math/geometry/angle.hpp" + +namespace tap::motor +{ +/** + * A class designed to interface with the encoder for DJI brand motors and motor controllers over + * CAN. This includes the C610 and C620 motor controllers and the GM6020 motor (that has a built-in + * motor controller). + * + * @note: the default positive rotation direction (i.e.: when `this->isMotorInverted() + * == false`) is counter clockwise when looking at the shaft from the side opposite + * the motor. This is specified in the C620 user manual (page 18). + * + * DJI motor encoders store a consistent encoding for a given angle across power-cycles. + * This means the encoder angle reported by the motor can have meaning if the encoding + * for an angle is unique as it is for the GM6020s. However for geared motors like the + * M3508 where a full encoder revolution does not correspond 1:1 to a shaft revolution, + * it is impossible to know the orientation of the shaft given just the encoder value. + * + * Combining them with some form of absolute encoder on the output shaft would give you knowledge of + * the orientation of the output shaft. + */ +class DjiMotorEncoder : public tap::encoder::WrappedEncoder +{ +public: + // 0 - 8191 for dji motors + static constexpr uint16_t ENC_RESOLUTION = 8192; + + // Internal gear ratio of the following motors. Output / Input + static constexpr float GEAR_RATIO_M3508 = 187.0f / 3591.0f; + static constexpr float GEAR_RATIO_GM3510_L1 = 1.0f / 3.7f; + static constexpr float GEAR_RATIO_GM3510_L2 = 1.0f / 5.2f; + static constexpr float GEAR_RATIO_GM3510_L3 = 1.0f / 19.0f; + static constexpr float GEAR_RATIO_GM3510_L4 = 1.0f / 27.0f; + static constexpr float GEAR_RATIO_M2006 = 1.0f / 36.0f; + static constexpr float GEAR_RATIO_GM6020 = 1.0f / 1.0f; + + /** + * @param isInverted if `false` the positive rotation direction of the shaft is + * counter-clockwise when looking at the shaft from. + * If `true` then the positive rotation direction will be clockwise. + * @param encoderResolution the number of encoder ticks before the value wraps. + * @param gearRatio the ratio of input revolutions to output revolutions of this encoder. + * @param encoderHomePosition the zero position for the encoder in encoder ticks. + */ + DjiMotorEncoder(bool isInverted, float gearRatio = 1, uint32_t encoderHomePosition = 0); + + void initialize() override{}; + + bool isOnline() const override; + + float getVelocity() const override; + + /** + * The current RPM reported by the motor controller. + */ + mockable int16_t getShaftRPM() const; + + DISALLOW_COPY_AND_ASSIGN(DjiMotorEncoder) + + /** + * Overrides virtual method in the can class, called every time a message with the + * CAN message id this class is attached to is received by the can receive handler. + * Parses the data in the message and updates this class's fields accordingly. + * + * @param[in] message the message to be processed. + */ + mockable void processMessage(const modm::can::Message& message); + +private: + // wait time before the motor is considered disconnected, in milliseconds + static const uint32_t MOTOR_DISCONNECT_TIME = 100; + + tap::arch::MilliTimeout encoderDisconnectTimeout; + + int16_t shaftRPM; +}; + +} // namespace tap::motor + +#endif // TAPROOT_DJI_MOTOR_ENCODER_HPP_ diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp new file mode 100644 index 00000000..01848237 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp @@ -0,0 +1,50 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_DJI_MOTOR_IDS_HPP_ +#define TAPROOT_DJI_MOTOR_IDS_HPP_ + +#include + +namespace tap::motor +{ +/** + * CAN IDs for the feedback messages sent by DJI motor controllers. Motor `i` in the set + * {1, 2,...,8} sends feedback data with in a CAN message with ID 0x200 + `i`. + * for declaring a new motor, must be one of these motor + * identifiers + */ +enum MotorId : uint32_t +{ + MOTOR1 = 0X201, + MOTOR2 = 0x202, + MOTOR3 = 0x203, + MOTOR4 = 0x204, + MOTOR5 = 0x205, + MOTOR6 = 0x206, + MOTOR7 = 0x207, + MOTOR8 = 0x208, +}; +} // namespace tap::motor + +#endif // TAPROOT_DJI_MOTOR_IDS_HPP_ diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp index 85d6a1d8..32518082 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp @@ -26,6 +26,7 @@ #include "tap/algorithms/strtok.hpp" #include "tap/drivers.hpp" +#include "dji_motor.hpp" #include "dji_motor_tx_handler.hpp" namespace tap @@ -190,7 +191,8 @@ void DjiMotorTerminalSerialHandler::getMotorInfoToString( { outputStream << (DJI_MOTOR_TO_NORMALIZED_ID(motor->getMotorIdentifier()) + 1) << ". " << motor->getName() << ": online: " << (motor->isMotorOnline() ? "yes" : "no") - << ", enc: " << motor->getEncoderWrapped() << ", rpm: " << motor->getShaftRPM() + << ", enc: " << motor->getInternalEncoder().getEncoder().getWrappedValue() + << ", rpm: " << motor->getInternalEncoder().getShaftRPM() << ", out des: " << motor->getOutputDesired() << modm::endl; } } diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp index cd887156..7b95afde 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp @@ -27,13 +27,14 @@ #include "tap/communication/serial/terminal_serial.hpp" #include "tap/util_macros.hpp" -#include "dji_motor.hpp" +#include "dji_motor_ids.hpp" namespace tap { class Drivers; namespace motor { +class DjiMotor; class DjiMotorTxHandler; class DjiMotorTerminalSerialHandler : public communication::serial::TerminalSerialCallbackInterface { diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp index 6c8cc2fb..a511fe64 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp @@ -32,6 +32,8 @@ #include "modm/architecture/interface/assert.h" #include "modm/architecture/interface/can_message.hpp" +#include "dji_motor.hpp" + namespace tap::motor { void DjiMotorTxHandler::addMotorToManager(DjiMotor** canMotorStore, DjiMotor* const motor) @@ -72,6 +74,11 @@ void DjiMotorTxHandler::encodeAndSendCanData() CAN_DJI_MESSAGE_SEND_LENGTH, 0, false); + modm::can::Message can1Message6020Current( + CAN_DJI_6020_CURRENT_IDENTIFIER, + CAN_DJI_MESSAGE_SEND_LENGTH, + 0, + false); modm::can::Message can2MessageLow( CAN_DJI_LOW_IDENTIFIER, CAN_DJI_MESSAGE_SEND_LENGTH, @@ -82,25 +89,36 @@ void DjiMotorTxHandler::encodeAndSendCanData() CAN_DJI_MESSAGE_SEND_LENGTH, 0, false); + modm::can::Message can2Message6020Current( + CAN_DJI_6020_CURRENT_IDENTIFIER, + CAN_DJI_MESSAGE_SEND_LENGTH, + 0, + false); bool can1ValidMotorMessageLow = false; bool can1ValidMotorMessageHigh = false; + bool can1ValidMotorMessage6020Current = false; bool can2ValidMotorMessageLow = false; bool can2ValidMotorMessageHigh = false; + bool can2ValidMotorMessage6020Current = false; serializeMotorStoreSendData( can1MotorStore, &can1MessageLow, &can1MessageHigh, + &can1Message6020Current, &can1ValidMotorMessageLow, - &can1ValidMotorMessageHigh); + &can1ValidMotorMessageHigh, + &can1ValidMotorMessage6020Current); serializeMotorStoreSendData( can2MotorStore, &can2MessageLow, &can2MessageHigh, + &can2Message6020Current, &can2ValidMotorMessageLow, - &can2ValidMotorMessageHigh); + &can2ValidMotorMessageHigh, + &can2ValidMotorMessage6020Current); bool messageSuccess = true; @@ -114,6 +132,11 @@ void DjiMotorTxHandler::encodeAndSendCanData() { messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1MessageHigh); } + if (can1ValidMotorMessage6020Current) + { + messageSuccess &= + drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1Message6020Current); + } } if (drivers->can.isReadyToSend(can::CanBus::CAN_BUS2)) { @@ -125,6 +148,11 @@ void DjiMotorTxHandler::encodeAndSendCanData() { messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS2, can2MessageHigh); } + if (can2ValidMotorMessage6020Current) + { + messageSuccess &= + drivers->can.sendMessage(can::CanBus::CAN_BUS2, can2Message6020Current); + } } if (!messageSuccess) @@ -137,8 +165,10 @@ void DjiMotorTxHandler::serializeMotorStoreSendData( DjiMotor** canMotorStore, modm::can::Message* messageLow, modm::can::Message* messageHigh, + modm::can::Message* message6020Current, bool* validMotorMessageLow, - bool* validMotorMessageHigh) + bool* validMotorMessageHigh, + bool* validMotorMessage6020Current) { for (int i = 0; i < DJI_MOTORS_PER_CAN; i++) { @@ -151,6 +181,11 @@ void DjiMotorTxHandler::serializeMotorStoreSendData( motor->serializeCanSendData(messageLow); *validMotorMessageLow = true; } + else if (motor->isInCurrentControl()) + { + motor->serializeCanSendData(message6020Current); + *validMotorMessage6020Current = true; + } else { motor->serializeCanSendData(messageHigh); diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp index 2807a55d..7ff4b2fa 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp @@ -28,7 +28,9 @@ #include "tap/util_macros.hpp" -#include "dji_motor.hpp" +#include "modm/architecture/interface/can_message.hpp" + +#include "dji_motor_ids.hpp" namespace tap { @@ -37,6 +39,8 @@ class Drivers; namespace tap::motor { +class DjiMotor; + /** * Converts the dji MotorId to a uint32_t. * @param[in] id Some CAN MotorId @@ -77,6 +81,8 @@ class DjiMotorTxHandler static constexpr uint32_t CAN_DJI_LOW_IDENTIFIER = 0X200; /** CAN message identifier for "high" segment (high 4 CAN motor IDs) of control message. */ static constexpr uint32_t CAN_DJI_HIGH_IDENTIFIER = 0X1FF; + /** CAN message identifier for 6020s in current mode of control message. */ + static constexpr uint32_t CAN_DJI_6020_CURRENT_IDENTIFIER = 0x1FE; DjiMotorTxHandler(Drivers* drivers) : drivers(drivers) {} mockable ~DjiMotorTxHandler() = default; @@ -116,8 +122,10 @@ class DjiMotorTxHandler DjiMotor** canMotorStore, modm::can::Message* messageLow, modm::can::Message* messageHigh, + modm::can::Message* message6020Current, bool* validMotorMessageLow, - bool* validMotorMessageHigh); + bool* validMotorMessageHigh, + bool* validMotorMessage6020Current); void removeFromMotorManager(const DjiMotor& motor, DjiMotor** motorStore); }; diff --git a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp index 3b0112fd..aa8a196c 100644 --- a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp +++ b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp @@ -23,6 +23,10 @@ #include "double_dji_motor.hpp" +#define CAST_ENC(x) \ + const_cast( \ + static_cast(&x)) + namespace tap::motor { DoubleDjiMotor::DoubleDjiMotor( @@ -35,24 +39,32 @@ DoubleDjiMotor::DoubleDjiMotor( bool isInvertedTwo, const char* nameOne, const char* nameTwo, - uint16_t encWrapped, - int64_t encRevolutions) + bool currentControl, + float gearRatio, + uint32_t encoderHomePositionOne, + tap::encoder::EncoderInterface* externalEncoder) : motorOne( drivers, desMotorIdentifierOne, motorCanBusOne, isInvertedOne, nameOne, - encWrapped, - encRevolutions), + currentControl, + gearRatio, + encoderHomePositionOne), motorTwo( drivers, desMotorIdentifierTwo, motorCanBusTwo, isInvertedTwo, nameTwo, - encWrapped, - encRevolutions) + currentControl, + gearRatio), + encoder( + {externalEncoder != nullptr ? externalEncoder : CAST_ENC(motorOne.getInternalEncoder()), + externalEncoder != nullptr ? CAST_ENC(motorOne.getInternalEncoder()) + : CAST_ENC(motorTwo.getInternalEncoder()), + externalEncoder != nullptr ? CAST_ENC(motorTwo.getInternalEncoder()) : nullptr}) { } @@ -60,22 +72,11 @@ void DoubleDjiMotor::initialize() { motorOne.initialize(); motorTwo.initialize(); + // This is weird because the initialize is called twice for the internal encoders. This is + // fine because the internal encoders have no initialize logic. + encoder.initialize(); } -int64_t DoubleDjiMotor::getEncoderUnwrapped() const { return motorOne.getEncoderUnwrapped(); } - -uint16_t DoubleDjiMotor::getEncoderWrapped() const { return motorOne.getEncoderWrapped(); } - -void DoubleDjiMotor::resetEncoderValue() -{ - motorOne.resetEncoderValue(); - motorTwo.resetEncoderValue(); -} - -float DoubleDjiMotor::getPositionUnwrapped() const { return motorOne.getPositionUnwrapped(); } - -float DoubleDjiMotor::getPositionWrapped() const { return motorOne.getPositionWrapped(); } - void DoubleDjiMotor::setDesiredOutput(int32_t desiredOutput) { motorOne.setDesiredOutput(desiredOutput); @@ -101,17 +102,13 @@ int8_t DoubleDjiMotor::getTemperature() const { return std::max(motorOne.getTemperature(), motorTwo.getTemperature()); } + int16_t DoubleDjiMotor::getTorque() const { - return (static_cast(motorOne.getTorque()) + - static_cast(motorTwo.getTorque())) / - 2; -} + int32_t m1Torque = motorOne.getTorque(); + int32_t m2Torque = motorTwo.getTorque(); + int num_online = motorOne.isMotorOnline() + motorTwo.isMotorOnline(); -int16_t DoubleDjiMotor::getShaftRPM() const -{ - return (static_cast(motorOne.getShaftRPM()) + - static_cast(motorTwo.getShaftRPM())) / - 2; + return num_online == 0 ? 0 : (m1Torque + m2Torque) / num_online; } } // namespace tap::motor diff --git a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp index c684fb26..c28d1719 100644 --- a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp +++ b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp @@ -54,21 +54,21 @@ class DoubleDjiMotor : public MotorInterface bool isInvertedTwo, const char* nameOne, const char* nameTwo, - uint16_t encWrapped = DjiMotor::ENC_RESOLUTION / 2, - int64_t encRevolutions = 0); + bool currentControl = false, + float gearRatio = 1, + uint32_t encoderHomePositionOne = 0, + tap::encoder::EncoderInterface* externalEncoder = nullptr); void initialize() override; - float getPositionUnwrapped() const override; - float getPositionWrapped() const override; - int64_t getEncoderUnwrapped() const override; - uint16_t getEncoderWrapped() const override; - void resetEncoderValue() override; + tap::encoder::EncoderInterface* getEncoder() const override + { + return const_cast*>(&this->encoder); + } void setDesiredOutput(int32_t desiredOutput) override; bool isMotorOnline() const override; int16_t getOutputDesired() const override; int8_t getTemperature() const override; int16_t getTorque() const override; - int16_t getShaftRPM() const override; protected: #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS) @@ -81,6 +81,7 @@ class DoubleDjiMotor : public MotorInterface DjiMotor motorOne; DjiMotor motorTwo; #endif + tap::encoder::MultiEncoder<3> encoder; }; } // namespace tap::motor diff --git a/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp b/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp index fba27b78..48901143 100644 --- a/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp +++ b/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp @@ -26,23 +26,34 @@ #include +#include "tap/communication/sensors/encoder/encoder_interface.hpp" + namespace tap::motor { class MotorInterface { public: virtual void initialize() = 0; - virtual int64_t getEncoderUnwrapped() const = 0; - virtual uint16_t getEncoderWrapped() const = 0; - virtual void resetEncoderValue() = 0; - virtual float getPositionUnwrapped() const = 0; - virtual float getPositionWrapped() const = 0; + virtual tap::encoder::EncoderInterface* getEncoder() const = 0; virtual void setDesiredOutput(int32_t desiredOutput) = 0; virtual bool isMotorOnline() const = 0; virtual int16_t getOutputDesired() const = 0; virtual int8_t getTemperature() const = 0; virtual int16_t getTorque() const = 0; - virtual int16_t getShaftRPM() const = 0; + + [[deprecated]] virtual void resetEncoderValue() { this->getEncoder()->resetEncoderValue(); }; + [[deprecated]] virtual float getPositionUnwrapped() const + { + return this->getEncoder()->getPosition().getUnwrappedValue(); + }; + [[deprecated]] virtual float getPositionWrapped() const + { + return this->getEncoder()->getPosition().getWrappedValue(); + }; + [[deprecated]] virtual int16_t getShaftRPM() const + { + return this->getEncoder()->getVelocity() / static_cast(M_TWOPI) * 60.f; + }; }; } // namespace tap::motor diff --git a/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp b/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp index 53a565d1..72c8ff36 100644 --- a/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp +++ b/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp @@ -29,7 +29,7 @@ #include #include -#include "tap/motor/dji_motor.hpp" +#include "tap/motor/dji_motor_ids.hpp" namespace modm::can { diff --git a/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp new file mode 100644 index 00000000..40e99f24 --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp @@ -0,0 +1,30 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "abstract_imu_mock.hpp" + +namespace tap::mock +{ +AbstractImuMock::AbstractImuMock() {} +AbstractImuMock::~AbstractImuMock() {} +} // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp new file mode 100644 index 00000000..40f4ceda --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp @@ -0,0 +1,54 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2025 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_ABSTRACT_IMU_MOCK_HPP_ +#define TAPROOT_ABSTRACT_IMU_MOCK_HPP_ + +#include + +#include "tap/communication/sensors/imu/abstract_imu.hpp" + +namespace tap::mock +{ +class AbstractImuMock : public tap::communication::sensors::imu::AbstractIMU +{ +public: + AbstractImuMock(); + virtual ~AbstractImuMock(); + + MOCK_METHOD(const char *, getName, (), (const override)); + MOCK_METHOD(float, getTemp, (), (const override)); + MOCK_METHOD(float, getAx, (), (const override)); + MOCK_METHOD(float, getAy, (), (const override)); + MOCK_METHOD(float, getAz, (), (const override)); + MOCK_METHOD(float, getGx, (), (const override)); + MOCK_METHOD(float, getGy, (), (const override)); + MOCK_METHOD(float, getGz, (), (const override)); + MOCK_METHOD(float, getYaw, (), (const override)); + MOCK_METHOD(float, getPitch, (), (const override)); + MOCK_METHOD(float, getRoll, (), (const override)); + MOCK_METHOD(float, getAccelerationSensitivity, (), (const override)); +}; +} // namespace tap::mock + +#endif // TAPROOT_ABSTRACT_IMU_MOCK_HPP_ diff --git a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp index 157a9bdc..a6ab2817 100644 --- a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp @@ -38,19 +38,18 @@ class Bmi088Mock : public tap::communication::sensors::imu::bmi088::Bmi088 MOCK_METHOD(void, initialize, (float, float, float), (override)); MOCK_METHOD(void, periodicIMUUpdate, (), (override)); - MOCK_METHOD(void, read, (), (override)); - MOCK_METHOD(ImuState, getImuState, (), (const override)); - MOCK_METHOD(void, requestRecalibration, (), (override)); - MOCK_METHOD(float, getYaw, (), (override)); - MOCK_METHOD(float, getPitch, (), (override)); - MOCK_METHOD(float, getRoll, (), (override)); - MOCK_METHOD(float, getGx, (), (override)); - MOCK_METHOD(float, getGy, (), (override)); - MOCK_METHOD(float, getGz, (), (override)); - MOCK_METHOD(float, getAx, (), (override)); - MOCK_METHOD(float, getAy, (), (override)); - MOCK_METHOD(float, getAz, (), (override)); - MOCK_METHOD(float, getTemp, (), (override)); + MOCK_METHOD(bool, read, (), (override)); + MOCK_METHOD(ImuState, getImuState, (), (const final override)); + MOCK_METHOD(float, getYaw, (), (const override)); + MOCK_METHOD(float, getPitch, (), (const override)); + MOCK_METHOD(float, getRoll, (), (const override)); + MOCK_METHOD(float, getGx, (), (const override)); + MOCK_METHOD(float, getGy, (), (const override)); + MOCK_METHOD(float, getGz, (), (const override)); + MOCK_METHOD(float, getAx, (), (const override)); + MOCK_METHOD(float, getAy, (), (const override)); + MOCK_METHOD(float, getAz, (), (const override)); + MOCK_METHOD(float, getTemp, (), (const override)); MOCK_METHOD(uint32_t, getPrevIMUDataReceivedTime, (), (const override)); }; } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp new file mode 100644 index 00000000..a2a88529 --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp @@ -0,0 +1,50 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "dji_motor_encoder_mock.hpp" + +namespace tap::mock +{ +DjiMotorEncoderMock::DjiMotorEncoderMock( + bool isInverted, + float gearRatio, + uint32_t encoderHomePosition) + : DjiMotorEncoder(isInverted, gearRatio, encoderHomePosition) +{ + ON_CALL(*this, isOnline).WillByDefault(testing::Return(true)); + ON_CALL(*this, getVelocity).WillByDefault(testing::Invoke([&]() { + return this->DjiMotorEncoder::getVelocity(); + })); + ON_CALL(*this, getPosition).WillByDefault(testing::Invoke([&]() { + return this->DjiMotorEncoder::getPosition(); + })); + ON_CALL(*this, getEncoder) + .WillByDefault(testing::Return(tap::algorithms::WrappedFloat(0, 0, ENC_RESOLUTION))); +} + +DjiMotorEncoderMock::~DjiMotorEncoderMock() +{ + ON_CALL(*this, getVelocity).WillByDefault(testing::Return(0)); + ON_CALL(*this, getPosition).WillByDefault(testing::Return(tap::algorithms::Angle(0))); +} +} // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp new file mode 100644 index 00000000..e9ab923c --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp @@ -0,0 +1,60 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_ +#define TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_ + +#include + +#include "tap/motor/dji_motor_encoder.hpp" + +#include "modm/architecture/interface/can_message.hpp" + +namespace tap::mock +{ +class DjiMotorEncoderMock : public tap::motor::DjiMotorEncoder +{ +public: + DjiMotorEncoderMock(bool isInverted, float gearRatio = 1, uint32_t encoderHomePosition = 0); + virtual ~DjiMotorEncoderMock(); + + MOCK_METHOD(void, initialize, (), (override)); + + MOCK_METHOD(bool, isOnline, (), (const override)); + + MOCK_METHOD(tap::algorithms::WrappedFloat, getPosition, (), (const override)); + + MOCK_METHOD(float, getVelocity, (), (const override)); + + MOCK_METHOD(tap::algorithms::WrappedFloat, getEncoder, (), (const)); + + MOCK_METHOD(int16_t, getShaftRPM, (), (const override)); + + MOCK_METHOD(void, resetEncoderValue, (), (override)); + + MOCK_METHOD(void, processMessage, (const modm::can::Message& message), (override)); +}; + +} // namespace tap::mock + +#endif // TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_ diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp index 7daac1e4..6815f9b0 100644 --- a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp +++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp @@ -31,16 +31,20 @@ DjiMotorMock::DjiMotorMock( tap::can::CanBus motorCanBus, bool isInverted, const char *name, - uint16_t encWrapped, - int64_t encRevolutions) + bool currentControl, + float gearRatio, + uint32_t encoderHomePosition, + tap::encoder::EncoderInterface *externalEncoder) : DjiMotor( drivers, desMotorIdentifier, motorCanBus, isInverted, name, - encWrapped, - encRevolutions) + currentControl, + gearRatio, + encoderHomePosition, + externalEncoder) { } DjiMotorMock::~DjiMotorMock() {} diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp index 50abfba2..9d4fb57c 100644 --- a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp @@ -31,6 +31,8 @@ #include "modm/architecture/interface/can_message.hpp" +#include "dji_motor_encoder_mock.hpp" + namespace tap { namespace mock @@ -44,26 +46,24 @@ class DjiMotorMock : public tap::motor::DjiMotor tap::can::CanBus motorCanBus, bool isInverted, const char* name, - uint16_t encWrapped = ENC_RESOLUTION / 2, - int64_t encRevolutions = 0); + bool currentControl = false, + float gearRatio = 1, + uint32_t encoderHomePosition = 0, + tap::encoder::EncoderInterface* externalEncoder = nullptr); virtual ~DjiMotorMock(); MOCK_METHOD(void, initialize, (), (override)); - MOCK_METHOD(float, getPositionUnwrapped, (), (const override)); - MOCK_METHOD(float, getPositionWrapped, (), (const override)); - MOCK_METHOD(int64_t, getEncoderUnwrapped, (), (const override)); - MOCK_METHOD(uint16_t, getEncoderWrapped, (), (const override)); MOCK_METHOD(void, processMessage, (const modm::can::Message& message), (override)); MOCK_METHOD(void, setDesiredOutput, (int32_t desiredOutput), (override)); MOCK_METHOD(void, resetEncoderValue, (), (override)); MOCK_METHOD(bool, isMotorOnline, (), (const override)); + MOCK_METHOD(bool, isInCurrentControl, (), (const override)); MOCK_METHOD(void, serializeCanSendData, (modm::can::Message * txMessage), (const override)); MOCK_METHOD(int16_t, getOutputDesired, (), (const override)); MOCK_METHOD(uint32_t, getMotorIdentifier, (), (const override)); MOCK_METHOD(int8_t, getTemperature, (), (const override)); MOCK_METHOD(int16_t, getTorque, (), (const override)); - MOCK_METHOD(int16_t, getShaftRPM, (), (const override)); MOCK_METHOD(bool, isMotorInverted, (), (const override)); MOCK_METHOD(tap::can::CanBus, getCanBus, (), (const override)); MOCK_METHOD(const char*, getName, (), (const override)); diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp index aa8eaba5..cea7fe7f 100644 --- a/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp +++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp @@ -23,6 +23,8 @@ #include "dji_motor_tx_handler_mock.hpp" +#include "tap/motor/dji_motor.hpp" + namespace tap::mock { DjiMotorTxHandlerMock::DjiMotorTxHandlerMock(tap::Drivers *drivers) diff --git a/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp new file mode 100644 index 00000000..89be60ff --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp @@ -0,0 +1,30 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#include "encoder_interface_mock.hpp" + +namespace tap::mock +{ +EncoderInterfaceMock::EncoderInterfaceMock() : EncoderInterface() {} +EncoderInterfaceMock::~EncoderInterfaceMock() {} +} // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp new file mode 100644 index 00000000..e8d709a8 --- /dev/null +++ b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp @@ -0,0 +1,54 @@ +/*****************************************************************************/ +/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ +/*****************************************************************************/ + +/* + * Copyright (c) 2024 Advanced Robotics at the University of Washington + * + * This file is part of Taproot. + * + * Taproot is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Taproot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Taproot. If not, see . + */ + +#ifndef TAPROOT_ENCODER_INTERFACE_MOCK_HPP_ +#define TAPROOT_ENCODER_INTERFACE_MOCK_HPP_ + +#include + +#include "tap/communication/sensors/encoder/encoder_interface.hpp" + +namespace tap::mock +{ +class EncoderInterfaceMock : public tap::encoder::EncoderInterface +{ +public: + EncoderInterfaceMock(); + virtual ~EncoderInterfaceMock(); + + MOCK_METHOD(void, initialize, (), (override)); + + MOCK_METHOD(bool, isOnline, (), (const override)); + + MOCK_METHOD(tap::algorithms::WrappedFloat, getPosition, (), (const override)); + + MOCK_METHOD(float, getVelocity, (), (const override)); + + MOCK_METHOD(void, resetEncoderValue, (), (override)); + + MOCK_METHOD(void, alignWith, (EncoderInterface*), (override)); +}; + +} // namespace tap::mock + +#endif // TAPROOT_ENCODER_INTERFACE_MOCK_HPP_ diff --git a/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp index 30c7a2c8..e98a56da 100644 --- a/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp @@ -37,16 +37,15 @@ class ImuInterfaceMock : public tap::communication::sensors::imu::ImuInterface virtual ~ImuInterfaceMock(); MOCK_METHOD(const char *, getName, (), (const override)); - MOCK_METHOD(float, getAx, (), (override)); - MOCK_METHOD(float, getAy, (), (override)); - MOCK_METHOD(float, getAz, (), (override)); - MOCK_METHOD(float, getGx, (), (override)); - MOCK_METHOD(float, getGy, (), (override)); - MOCK_METHOD(float, getGz, (), (override)); - MOCK_METHOD(float, getTemp, (), (override)); - MOCK_METHOD(float, getYaw, (), (override)); - MOCK_METHOD(float, getPitch, (), (override)); - MOCK_METHOD(float, getRoll, (), (override)); + MOCK_METHOD(float, getAx, (), (const override)); + MOCK_METHOD(float, getAy, (), (const override)); + MOCK_METHOD(float, getAz, (), (const override)); + MOCK_METHOD(float, getGx, (), (const override)); + MOCK_METHOD(float, getGy, (), (const override)); + MOCK_METHOD(float, getGz, (), (const override)); + MOCK_METHOD(float, getYaw, (), (const override)); + MOCK_METHOD(float, getPitch, (), (const override)); + MOCK_METHOD(float, getRoll, (), (const override)); }; } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp index 5f5154bc..b62f9a05 100644 --- a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp +++ b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp @@ -27,7 +27,7 @@ namespace tap::mock { ImuTerminalSerialHandlerMock::ImuTerminalSerialHandlerMock( tap::Drivers* drivers, - communication::sensors::imu::ImuInterface* imu) + communication::sensors::imu::AbstractIMU* imu) : communication::sensors::imu::ImuTerminalSerialHandler(drivers, imu) { } diff --git a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp index e9927c78..6f18693d 100644 --- a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp @@ -35,7 +35,7 @@ class ImuTerminalSerialHandlerMock : public communication::sensors::imu::ImuTerm public: ImuTerminalSerialHandlerMock( tap::Drivers* drivers, - communication::sensors::imu::ImuInterface* imu); + communication::sensors::imu::AbstractIMU* imu); virtual ~ImuTerminalSerialHandlerMock(); MOCK_METHOD(void, init, (), (override)); diff --git a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp index 3db31015..0b4c4e4c 100644 --- a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp +++ b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp @@ -25,7 +25,7 @@ namespace tap::mock { -MotorInterfaceMock::MotorInterfaceMock() : tap::motor::MotorInterface() {} +MotorInterfaceMock::MotorInterfaceMock() : tap::motor::MotorInterface(), encoder() {} MotorInterfaceMock::~MotorInterfaceMock() {} } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp index 4586473c..7b27a1dc 100644 --- a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp @@ -30,6 +30,8 @@ #include "tap/motor/motor_interface.hpp" +#include "encoder_interface_mock.hpp" + namespace tap::mock { class MotorInterfaceMock : public tap::motor::MotorInterface @@ -39,17 +41,19 @@ class MotorInterfaceMock : public tap::motor::MotorInterface virtual ~MotorInterfaceMock(); MOCK_METHOD(void, initialize, (), (override)); - MOCK_METHOD(int64_t, getEncoderUnwrapped, (), (const override)); - MOCK_METHOD(uint16_t, getEncoderWrapped, (), (const override)); - MOCK_METHOD(void, resetEncoderValue, (), (override)); - MOCK_METHOD(float, getPositionUnwrapped, (), (const override)); - MOCK_METHOD(float, getPositionWrapped, (), (const override)); MOCK_METHOD(void, setDesiredOutput, (int32_t), (override)); MOCK_METHOD(bool, isMotorOnline, (), (const override)); MOCK_METHOD(int16_t, getOutputDesired, (), (const override)); MOCK_METHOD(int8_t, getTemperature, (), (const override)); MOCK_METHOD(int16_t, getTorque, (), (const override)); - MOCK_METHOD(int16_t, getShaftRPM, (), (const override)); + + EncoderInterfaceMock* getEncoder() const override + { + return const_cast*>(&encoder); + } + +private: + testing::NiceMock encoder; }; } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp index 7fa3f649..60cf4016 100644 --- a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp @@ -37,6 +37,12 @@ class Odometry2DInterfaceMock : public algorithms::odometry::Odometry2DInterface MOCK_METHOD(modm::Vector2f, getCurrentVelocity2D, (), (const override)); MOCK_METHOD(uint32_t, getLastComputedOdometryTime, (), (const override)); MOCK_METHOD(float, getYaw, (), (const override)); + MOCK_METHOD( + void, + overrideOdometryPosition, + (const float positionX, const float positionY), + (override)); + MOCK_METHOD(void, reset, (), (override)); }; } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp b/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp index d0b595d5..58b76f37 100644 --- a/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp @@ -54,7 +54,7 @@ class RefSerialMock : public tap::communication::serial::RefSerial (override)); MOCK_METHOD(RobotId, getRobotIdBasedOnCurrentRobotTeam, (RobotId), (override)); MOCK_METHOD(bool, acquireTransmissionSemaphore, (), (override)); - MOCK_METHOD(void, releaseTransmissionSemaphore, (uint32_t), (override)); + MOCK_METHOD(void, releaseTransmissionSemaphore, (), (override)); }; // class RefSerialMock } // namespace mock } // namespace tap From ff8c2a04faaa19606c425879afd238aee5a2c14a Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 17 Jun 2025 01:52:35 -0500 Subject: [PATCH 11/19] regenerate taproot --- taproot | 2 +- .../src/tap/algorithms/butterworth.hpp | 435 +++++++++--------- .../src/tap/algorithms/discrete_filter.hpp | 49 +- .../odometry/odometry_2d_interface.hpp | 5 - .../src/tap/algorithms/wrapped_float.cpp | 12 +- .../sensors/encoder/wrapped_encoder.cpp | 15 +- .../sensors/encoder/wrapped_encoder.hpp | 4 +- .../sensors/imu/abstract_imu.cpp | 2 +- .../sensors/imu/abstract_imu.hpp | 10 +- .../sensors/imu/bmi088/bmi088.cpp | 5 +- .../sensors/imu/imu_interface.hpp | 7 - .../tap/communication/serial/ref_serial.hpp | 4 +- .../communication/serial/ref_serial_data.hpp | 11 +- .../serial/ref_serial_transmitter.cpp | 7 +- .../src/tap/motor/dji_motor_encoder.hpp | 14 +- .../tap/mock/odometry_2d_interface_mock.hpp | 1 - .../taproot/test/tap/mock/ref_serial_mock.hpp | 2 +- 17 files changed, 278 insertions(+), 307 deletions(-) diff --git a/taproot b/taproot index 2a6bf7c7..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 2a6bf7c7e30827d6aa210bee8125d2e7bc28862a +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd diff --git a/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp index d2f53634..6051dd71 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp @@ -30,12 +30,11 @@ #include "modm/math/geometry/angle.hpp" -#include "discrete_filter.hpp" - /** + * @class Butterworth * @brief Implementation of Butterworth filter design in the discrete domain. * - * This header file provides a implementation of Butterworth filters, + * This header file provides a comprehensive implementation of Butterworth filters, * including low-pass, high-pass, band-pass, and band-stop filters. The Butterworth * filter is known for its maximally flat frequency response in the passband, making * it ideal for applications requiring minimal signal distortion. @@ -72,7 +71,8 @@ * \f[ s = \frac{2}{T} \cdot \frac{z - 1}{z + 1} \f] * * @note - * This implementation is designed for C++20 ``constexpr``. + * This implementation is designed for C++20 ``constexpr``, enabling + * compile-time computation of filter configurations. * * @warning * High-order filters can introduce high phase delays and should be used with caution. @@ -89,8 +89,10 @@ * Then, pass those coefficients into a ``DiscreteFilter``. * * @code - * Coefficients coe = butterworth<1, LOWPASS>(wc, Ts); - * DiscreteFilter<2> Filter(coe); + * static constexpr Butterworth<1, LOWPASS> filter(wc, Ts); + * auto naturalCoeffs = filter.getNaturalResponseCoefficients(); + * auto forcedCoeffs = filter.getForcedResponseCoefficients(); + * DiscreteFilter<2> Filter(naturalCoeffs, forcedCoeffs); * * @endcode * @@ -99,7 +101,9 @@ * @version 2.0 */ -namespace tap::algorithms::filter +namespace tap +{ +namespace algorithms { enum FilterType : uint8_t { @@ -119,8 +123,7 @@ enum FilterType : uint8_t */ constexpr std::complex s2z(std::complex s, double Ts) { - return (static_cast>(1.0) + (Ts / 2) * s) / - (static_cast>(1.0) - (Ts / 2) * s); + return (1.0 + (Ts / 2) * s) / (1.0 - (Ts / 2) * s); } /** @@ -209,7 +212,7 @@ constexpr std::complex evaluateFrequencyResponse( * @param [in] z the complex number to calculate the magnitude of * @return the magnitude of the complex number */ -constexpr double complexAbs(std::complex z) +constexpr double complex_abs(std::complex z) { return std::sqrt(z.real() * z.real() + z.imag() * z.imag()); } @@ -220,248 +223,262 @@ constexpr double complexAbs(std::complex z) * @return the square root of the complex number */ -constexpr std::complex complexSqrt(std::complex z) +constexpr std::complex complex_sqrt(std::complex z) { - double r = std::sqrt(complexAbs(z)); - double theta = static_cast(std::atan2(z.imag(), z.real())) * static_cast(0.5f); + double r = std::sqrt(complex_abs(z)); + double theta = std::atan2(z.imag(), z.real()) * 0.5; return {r * std::cos(theta), r * std::sin(theta)}; } -constexpr uint16_t getNumCoefficients(uint8_t ORDER, FilterType type) -{ - return (1 + ((type & 0b10) != 0)) * ORDER + 1; -} - -/** - * @param[in] wc for LOW/HIGHPASS: cutoff ωc. - * for BANDPASS/BANDSTOP: lower edge ωl. - * @param[in] Ts sample time. - * @param[in] type filter type, LOWPASS, HIGHPASS, BANDPASS, BANDSTOP. - * defaults to LOWPASS. - * @param[in] wh upper edge ωh (only used for band filters). - */ template -Coefficients constexpr butterworth( - double wc, - double Ts, - double wh = 0.0) +class Butterworth { - const uint16_t COEFFICIENTS = getNumCoefficients(ORDER, Type); - - std::array naturalResponseCoefficients; - std::array forcedResponseCoefficients; - - const int n = ORDER; +public: + /** + * @param[in] wc for LOW/HIGHPASS: cutoff ωc. + * for BANDPASS/BANDSTOP: lower edge ωl. + * @param[in] Ts sample time. + * @param[in] type filter type, LOWPASS, HIGHPASS, BANDPASS, BANDSTOP. + * defaults to LOWPASS. + * @param[in] wh upper edge ωh (only used for band filters). + */ + constexpr Butterworth(double wc, double Ts, double wh = 0.0) + : naturalResponseCoefficients(), + forcedResponseCoefficients() + { + const int n = ORDER; - // For band filters we treat wc as ωl - double wl = wc; - double whp = wh; - std::array, 2 * ORDER> bandpass_stop_poles; + // For band filters we treat wc as ωl + double wl = wc; + double whp = wh; + std::array, 2 * ORDER> bandpass_stop_poles; - // pre-warp all edges for bilinear transform - wl = static_cast(2.0) / Ts * std::tan(wl * (Ts / static_cast(2.0))); - whp = static_cast(2.0) / Ts * std::tan(whp * (Ts / static_cast(2.0))); + // pre-warp all edges for bilinear transform + wl = (2.0 / Ts) * std::tan(wl * (Ts / 2.0)); + whp = (2.0 / Ts) * std::tan(whp * (Ts / 2.0)); - // generate N prototype poles on unit circle - std::array, COEFFICIENTS - 1> poles; - for (int k = 0; k < n; ++k) - { - double theta = M_PI * (2.0 * k + 1) / (2.0 * n) + M_PI / 2.0; - poles[k] = std::complex(std::cos(theta), std::sin(theta)); - } + // generate N prototype poles on unit circle + std::array, COEFFICIENTS - 1> poles; + for (int k = 0; k < n; ++k) + { + double theta = M_PI * (2.0 * k + 1) / (2.0 * n) + M_PI / 2.0; + poles[k] = std::complex(std::cos(theta), std::sin(theta)); + } - std::array, COEFFICIENTS - 1> zPoles; + std::array, COEFFICIENTS - 1> zPoles; - // apply the appropriate s-domaisn transform to each pole - switch (Type) - { - case LOWPASS: + // apply the appropriate s-domain transform to each pole + switch (Type) { - // poles are multiplied by wl to scale them to the desired cutoff frequency - for (int j = 0; j < n; ++j) poles[j] = poles[j] * wl; + case LOWPASS: + { + // poles are multiplied by wl to scale them to the desired cutoff frequency + for (int j = 0; j < n; ++j) poles[j] = poles[j] * wl; - // now map each analog pole into the z-plane - for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); - break; - } - case HIGHPASS: - { - // applys the inverse transform of the butterworth lowpass filter - for (int j = 0; j < n; ++j) + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); + break; + } + case HIGHPASS: { - poles[j] = wl / poles[j]; + // applys the inverse transform of the butterworth lowpass filter + for (int j = 0; j < n; ++j) + { + poles[j] = wl / poles[j]; + } + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); + break; } - // now map each analog pole into the z-plane - for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts); - break; - } - // In the case of a bandpass or a bandstop the amount of poles doubles. - case BANDPASS: - { - /* - * transform in the form of s → (s² + Ω₀²) / (B · s) - * where: - * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) - * B = Ω_high - Ω_low // Bandwidth (rad/sec) - */ - double B = whp - wl; - double W0sq = whp * wl; - - for (int j = 0; j < ORDER; ++j) + // In the case of a bandpass or a bandstop the amount of poles doubles. + case BANDPASS: { - std::complex p = poles[j]; - - std::complex discriminant = - (p * B) * (p * B) - static_cast>(4.0) * W0sq; - std::complex root = complexSqrt(discriminant); - - bandpass_stop_poles[2 * j] = - (p * B + root) * static_cast>(0.5); - bandpass_stop_poles[2 * j + 1] = - (p * B - root) * static_cast>(0.5); + // check for validity of the filter edges + if (whp <= wl) + { + throw std::invalid_argument("wh must be > wl for BANDPASS"); + } + /* + * transform in the form of s → (s² + Ω₀²) / (B · s) + * where: + * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) + * B = Ω_high - Ω_low // Bandwidth (rad/sec) + */ + double B = whp - wl; + double W0sq = whp * wl; + + for (int j = 0; j < ORDER; ++j) + { + std::complex p = poles[j]; + + std::complex discriminant = (p * B) * (p * B) - 4.0 * W0sq; + std::complex root = complex_sqrt(discriminant); + + bandpass_stop_poles[2 * j] = (p * B + root) * 0.5; + bandpass_stop_poles[2 * j + 1] = (p * B - root) * 0.5; + } + + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) + zPoles[i] = s2z(bandpass_stop_poles[i], Ts); + + break; } - // now map each analog pole into the z-plane - for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(bandpass_stop_poles[i], Ts); - - break; + case BANDSTOP: + { + // check for validity of the filter edges + if (whp <= wl) + { + throw std::invalid_argument("wh must be > wl for BANDSTOP"); + } + /* + * transform in the form of s → B · s / (s² + Ω₀²) + * where: + * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) + * B = Ω_high - Ω_low // Bandwidth (rad/sec) + */ + double B = whp - wl; + double W0sq = whp * wl; + for (int j = 0; j < n; ++j) + { + std::complex p = poles[j]; + + std::complex discriminant = B * B - (4.0 * -p * W0sq); + std::complex root = complex_sqrt(discriminant); + + bandpass_stop_poles[2 * j] = (B + root) / (2.0 * p); + bandpass_stop_poles[2 * j + 1] = (B - root) / (2.0 * p); + } + + // now map each analog pole into the z-plane + for (int i = 0; i < COEFFICIENTS - 1; ++i) + zPoles[i] = s2z(bandpass_stop_poles[i], Ts); + + break; + } + default: + throw std::invalid_argument("Unknown filter type"); } - case BANDSTOP: + std::array, COEFFICIENTS - 1> zZeros; + + switch (Type) { - /* - * transform in the form of s → B · s / (s² + Ω₀²) - * where: - * Ω₀ = √(Ω_low * Ω_high) // Center frequency (rad/sec) - * B = Ω_high - Ω_low // Bandwidth (rad/sec) - */ - double B = whp - wl; - double W0sq = whp * wl; - for (int j = 0; j < n; ++j) + case LOWPASS: + // zeros: for Butterworth lowpass all z-zeros at z = –1 + zZeros.fill(std::complex(-1, 0)); + break; + case HIGHPASS: + // zeros: for butterworth highpass all z-zeros are at z = 1 + zZeros.fill(std::complex(1, 0)); + break; + case BANDPASS: + // zeros: for butterworth bandpass all z-zeros are at z = ±1 + for (int i = 0; i < COEFFICIENTS - 1; ++i) + { + zZeros[i] = + (i % 2 == 0) ? std::complex(1, 0) : std::complex(-1, 0); + } + break; + case BANDSTOP: { - std::complex p = poles[j]; + /* zeros: for butterworth bandstop are in a circle with radius 1 at the + * center of the z-domain with an angle in radians per sample of the + * center frequency. The zeros are complex conjugates of each other. + */ - std::complex discriminant = - B * B - (static_cast>(4.0) * -p * W0sq); - std::complex root = complexSqrt(discriminant); + /* the notch (center) frequency in radians/sample */ + double omega0 = std::sqrt(wl * whp) * Ts; - bandpass_stop_poles[2 * j] = - (B + root) / (static_cast>(2.0) * p); - bandpass_stop_poles[2 * j + 1] = - (B - root) / (static_cast>(2.0) * p); - } + double realPart = std::cos(omega0); + double imagPart = std::sin(omega0); - // now map each analog pole into the z-plane - for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(bandpass_stop_poles[i], Ts); + std::complex zeroPlus(realPart, imagPart); // e^(+jω0) + std::complex zeroMinus(realPart, -imagPart); // e^(-jω0) + for (int i = 0; i < COEFFICIENTS - 1; i += 2) + { + zZeros[i] = zeroPlus; // first of pair + if (i + 1 < COEFFICIENTS) zZeros[i + 1] = zeroMinus; // second of pair + } + } break; } - } - std::array, COEFFICIENTS - 1> zZeros; + // get expanded polynomials + auto b = expandPolynomial(zZeros); + auto a = expandPolynomial(zPoles); - switch (Type) - { - case LOWPASS: - // zeros: for Butterworth lowpass all z-zeros at z = –1 - zZeros.fill(std::complex(-1, 0)); - break; - case HIGHPASS: - // zeros: for butterworth highpass all z-zeros are at z = 1 - zZeros.fill(std::complex(1, 0)); - break; - case BANDPASS: - // zeros: for butterworth bandpass all z-zeros are at z = ±1 - for (int i = 0; i < COEFFICIENTS - 1; ++i) + // scale the gain properly + switch (Type) + { + case LOWPASS: { - zZeros[i] = (i % 2 == 0) ? std::complex(1, 0) : std::complex(-1, 0); + // Eval at DC + auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); + auto mag = complex_abs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; } - break; - case BANDSTOP: - { - /* zeros: for butterworth bandstop are in a circle with radius 1 at the - * center of the z-domain with an angle in radians per sample of the - * center frequency. The zeros are complex conjugates of each other. - */ - - /* the notch (center) frequency in radians/sample */ - double omega0 = std::sqrt(wl * whp) * Ts; - - double realPart = std::cos(omega0); - double imagPart = std::sin(omega0); - - std::complex zeroPlus(realPart, imagPart); // e^(+jω0) - std::complex zeroMinus(realPart, -imagPart); // e^(-jω0) - - for (int i = 0; i < COEFFICIENTS - 1; i += 2) + case HIGHPASS: + { + // Eval at niquest + auto freqResp = evaluateFrequencyResponse(b, a, M_PI / Ts, Ts); + auto mag = complex_abs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + case BANDPASS: + { + // Eval at center frequency + auto freqResp = + evaluateFrequencyResponse(b, a, std::sqrt(wl * whp), Ts); + auto mag = complex_abs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + case BANDSTOP: { - zZeros[i] = zeroPlus; // first of pair - if (i + 1 < COEFFICIENTS) zZeros[i + 1] = zeroMinus; // second of pair + // Eval at dc gain + auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); + auto mag = complex_abs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; } } - break; + + // store in member arrays (reverse order) + for (size_t i = 0; i < COEFFICIENTS; ++i) + { + naturalResponseCoefficients[COEFFICIENTS - i - 1] = a[i]; + forcedResponseCoefficients[COEFFICIENTS - i - 1] = b[i]; + } } - // get expanded polynomials - auto b = expandPolynomial(zZeros); - auto a = expandPolynomial(zPoles); + static constexpr int COEFFICIENTS = (1 + ((Type & 0b10) != 0)) * ORDER + 1; - // scale the gain properly - switch (Type) + std::array getNaturalResponseCoefficients() const { - case LOWPASS: - { - // Eval at DC - auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); - auto mag = complexAbs(freqResp); - double scale = 1 / mag; - for (auto &coef : b) coef *= scale; - break; - } - case HIGHPASS: - { - // Eval at nyquist - auto freqResp = evaluateFrequencyResponse( - b, - a, - static_cast(M_PI) / Ts, - Ts); - auto mag = complexAbs(freqResp); - double scale = 1 / mag; - for (auto &coef : b) coef *= scale; - break; - } - case BANDPASS: - { - // Eval at center frequency - auto freqResp = - evaluateFrequencyResponse(b, a, std::sqrt(wl * whp), Ts); - auto mag = complexAbs(freqResp); - double scale = 1 / mag; - for (auto &coef : b) coef *= scale; - break; - } - case BANDSTOP: - { - // Eval at dc gain - auto freqResp = evaluateFrequencyResponse(b, a, 0, Ts); - auto mag = complexAbs(freqResp); - double scale = 1 / mag; - for (auto &coef : b) coef *= scale; - break; - } + return naturalResponseCoefficients; } - // store in member arrays (reverse order) - for (size_t i = 0; i < COEFFICIENTS; ++i) + std::array getForcedResponseCoefficients() const { - naturalResponseCoefficients[COEFFICIENTS - i - 1] = a[i]; - forcedResponseCoefficients[COEFFICIENTS - i - 1] = b[i]; + return forcedResponseCoefficients; } - return {naturalResponseCoefficients, forcedResponseCoefficients}; -} +private: + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; +}; + +} // namespace algorithms -} // namespace tap::algorithms::filter +} // namespace tap -#endif // TAPROOT_BUTTERWORTH_HPP_ +#endif // TAPROOT_BUTTERWORTH_HPP_ \ No newline at end of file diff --git a/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp index 8e69f207..363ad69b 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp @@ -27,26 +27,10 @@ #include #include -namespace tap::algorithms::filter +namespace tap { -/** - * @struct Coefficients - * @brief Represents the coefficients used in a discrete filter. - * - * This structure holds two sets of coefficients: - * - `naturalResponseCoefficients`: Coefficients related to the natural response of the system. - * - `forcedResponseCoefficients`: Coefficients related to the forced response of the system. - * - * @tparam T The data type of the coefficients (e.g., float, double). - * @tparam SIZE The size of the coefficient arrays. - */ -template -struct Coefficients +namespace algorithms { - std::array naturalResponseCoefficients; - std::array forcedResponseCoefficients; -}; - /** * @brief DiscreteFilter class implements a discrete-time filter using the finite difference * equation. @@ -68,21 +52,14 @@ class DiscreteFilter * state to zero. */ DiscreteFilter( - std::array naturalResponseCoefficients, - std::array forcedResponseCoefficients) + std::array &naturalResponseCoefficients, + std::array &forcedResponseCoefficients) : naturalResponseCoefficients(naturalResponseCoefficients), forcedResponseCoefficients(forcedResponseCoefficients) { reset(); } - DiscreteFilter(const Coefficients coefficients) - : naturalResponseCoefficients(coefficients.naturalResponseCoefficients), - forcedResponseCoefficients(coefficients.forcedResponseCoefficients) - { - reset(); - } - /** * @brief Filters the input data using the finite difference equation. * @param [in] dat The input data to be filtered. @@ -144,20 +121,6 @@ class DiscreteFilter return 0.0f; } - void setCoefficients(Coefficients coe) - { - this->naturalResponseCoefficients = coe.naturalResponseCoefficients; - this->forcedResponseCoefficients = coe.forcedResponseCoefficients; - } - - void setCoefficients( - std::array naturalResponseCoefficients, - std::array forcedResponseCoefficients) - { - this->naturalResponseCoefficients = naturalResponseCoefficients; - this->forcedResponseCoefficients = forcedResponseCoefficients; - } - private: std::array naturalResponseCoefficients; std::array forcedResponseCoefficients; @@ -165,6 +128,8 @@ class DiscreteFilter std::array forcedResponse; }; -} // namespace tap::algorithms::filter +} // namespace algorithms + +} // namespace tap #endif // TAPROOT_DISCRETE_FILTER_HPP_ \ No newline at end of file diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp index 3943d305..38c2f210 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp +++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp @@ -63,11 +63,6 @@ class Odometry2DInterface */ virtual uint32_t getLastComputedOdometryTime() const = 0; - /** - * Resets odometry to initial pose. - */ - virtual void reset() = 0; - virtual void overrideOdometryPosition(const float positionX, const float positionY) = 0; }; diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp index bc7c2c02..e040f579 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp +++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.cpp @@ -128,9 +128,15 @@ void WrappedFloat::shiftBounds(const float shiftMagnitude) void WrappedFloat::wrapValue() { float oldValue = wrapped; - float dRev = floor((oldValue - lowerBound) / (upperBound - lowerBound)); - this->revolutions += dRev; - this->wrapped = oldValue - (upperBound - lowerBound) * dRev; + if (oldValue < lowerBound) + { + this->wrapped = upperBound + fmodf(oldValue - upperBound, upperBound - lowerBound); + } + else if (oldValue >= upperBound) + { + this->wrapped = lowerBound + fmodf(oldValue - lowerBound, upperBound - lowerBound); + } + this->revolutions += floor((oldValue - lowerBound) / (upperBound - lowerBound)); } float WrappedFloat::limitValue( diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp index bb8c36fe..317ff7dc 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp @@ -80,7 +80,7 @@ void WrappedEncoder::alignWith(EncoderInterface* other) { tap::algorithms::WrappedFloat positionDifference = other->getPosition() - position; float offset = positionDifference.getUnwrappedValue() / static_cast(M_TWOPI) * - encoderResolution / gearRatio; + encoderResolution * gearRatio; this->encoderHomePosition += offset; this->encoder += offset; this->position = other->getPosition(); @@ -108,18 +108,9 @@ void WrappedEncoder::updateEncoderValue(uint32_t encoderActual) encoder += encoder.minDifference(newEncWrapped); } - uint32_t time = tap::arch::clock::getTimeMicroseconds(); - if (time < lastUpdateTime) - { - deltaTime = time + ((0xFFFFFFFFul) - lastUpdateTime); - } - else - { - deltaTime = time - lastUpdateTime; - } - lastUpdateTime = time; - pastPosition = position; + deltaTime = tap::arch::clock::getTimeMicroseconds() - lastUpdateTime; + lastUpdateTime = tap::arch::clock::getTimeMicroseconds(); position.setUnwrappedValue( encoder.getUnwrappedValue() * static_cast(M_TWOPI) / encoderResolution * gearRatio); } diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp index fd35eeb2..83b091ea 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp @@ -109,9 +109,9 @@ class WrappedEncoder : public EncoderInterface /** * The last update time for the encoder. Used in velocity calculations. */ - uint32_t lastUpdateTime; + uint64_t lastUpdateTime; - uint32_t deltaTime; + uint64_t deltaTime; }; } // namespace tap::encoder diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp index 732e25a2..ba33c9fe 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp @@ -43,7 +43,7 @@ void AbstractIMU::requestCalibration() void AbstractIMU::setMountingTransform(const Transform& transform) { - mountingTransform = transform.getInverse(); + mountingTransform = transform; } void AbstractIMU::periodicIMUUpdate() diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp index b91c6e86..84bb24c4 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp @@ -45,7 +45,7 @@ class AbstractIMU : public ImuInterface { public: AbstractIMU(const Transform& mountingTransform = Transform::identity()) - : mountingTransform(mountingTransform.getInverse()) + : mountingTransform(mountingTransform) { } @@ -61,7 +61,7 @@ class AbstractIMU : public ImuInterface * calibrating, angle, accelerometer, and gyroscope values will return 0. When calibrating * the BMI088 should be level, otherwise the IMU will be calibrated incorrectly. */ - virtual void requestCalibration() override; + virtual void requestCalibration(); /** * Call this function at same rate as intialized sample frequency. @@ -116,10 +116,10 @@ class AbstractIMU : public ImuInterface void setAccelOffset(float x, float y, float z); void setGyroOffset(float x, float y, float z); - inline void applyMountingTransformToRaw(ImuData& data) + inline void applyTransform(ImuData& data) { - data.accRaw = mountingTransform.apply(data.accRaw); - data.gyroRaw = mountingTransform.apply(data.gyroRaw); + data.accG = mountingTransform.apply(data.accG); + data.gyroRadPerSec = mountingTransform.apply(data.gyroRadPerSec); } virtual inline float getAccelerationSensitivity() const = 0; diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp index 8d95fab8..425613f3 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp @@ -175,14 +175,15 @@ bool Bmi088::read() float rawGyroZ = bigEndianInt16ToFloat(rxBuff + 4); imuData.gyroRaw = tap::algorithms::transforms::Vector(rawGyroX, rawGyroY, rawGyroZ); - applyMountingTransformToRaw(imuData); - Bmi088Hal::bmi088AccReadMultiReg(Acc::TEMP_MSB, rxBuff, 2); imuData.temperature = parseTemp(rxBuff[0], rxBuff[1]); imuData.gyroRadPerSec = (imuData.gyroRaw - imuData.gyroOffsetRaw) * GYRO_RAD_PER_S_PER_GYRO_COUNT; imuData.accG = (imuData.accRaw - imuData.accOffsetRaw) * ACC_G_PER_ACC_COUNT; + + applyTransform(imuData); + return true; } diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp index e0f8d942..905c104c 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp @@ -102,13 +102,6 @@ class ImuInterface * Returns roll angle in radians. */ virtual inline float getRoll() const = 0; - - /** - * When this function is called, the IMU enters a calibration state during which time, - * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When - * calibrating, angle, accelerometer, and gyroscope values will return 0. - */ - virtual void requestCalibration() = 0; }; } // namespace tap::communication::sensors::imu diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp index d35cad73..11c330f9 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp @@ -172,11 +172,11 @@ class RefSerial : public DJISerial, public RefSerialData return false; } - mockable void releaseTransmissionSemaphore() + mockable void releaseTransmissionSemaphore(uint32_t sentMsgLen) { transmissionSemaphore.release(); transmissionDelayTimer.restart( - std::ceil(1.0f / RefSerialData::Tx::ROBOT_INTERACTION_DATA_RATE * 1000.0f)); + std::ceil(sentMsgLen * 1000.0f / Tx::MAX_TRANSMIT_SPEED_BYTES_PER_S)); } /** diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp index 486b6ad3..8bae42f0 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp @@ -655,10 +655,13 @@ class RefSerialData } modm_packed; /** - * As defined as the rate (in hz) at which the 0x0301 message is sent to the referee - * system. + * You cannot send messages faster than this speed to the referee system. + * + * Source: https://bbs.robomaster.com/forum.php?mod=viewthread&tid=9120 + * + * Changed from 1280 to 1000 as the HUD was still unreliable. */ - static constexpr uint8_t ROBOT_INTERACTION_DATA_RATE = 30; + static constexpr uint32_t MAX_TRANSMIT_SPEED_BYTES_PER_S = 1000; /** * Get the min wait time after which you can send more data to the client. Sending faster @@ -686,7 +689,7 @@ class RefSerialData "Invalid type, getWaitTimeAfterGraphicSendMs only takes in ref serial message " "types."); - return 1'000 / ROBOT_INTERACTION_DATA_RATE; + return sizeof(T) * 1'000 / MAX_TRANSMIT_SPEED_BYTES_PER_S; } }; }; diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp index 9a474928..22272145 100644 --- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp +++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_transmitter.cpp @@ -234,7 +234,7 @@ modm::ResumableResult RefSerialTransmitter::deleteGraphicLayer( reinterpret_cast(&deleteGraphicLayerMessage), sizeof(Tx::DeleteGraphicLayerMessage)); - drivers->refSerial.releaseTransmissionSemaphore(); + drivers->refSerial.releaseTransmissionSemaphore(sizeof(Tx::DeleteGraphicLayerMessage)); RF_END(); } @@ -285,7 +285,7 @@ modm::ResumableResult RefSerialTransmitter::sendGraphic_( reinterpret_cast(graphicMsg), sizeof(*graphicMsg)); - drivers->refSerial.releaseTransmissionSemaphore(); + drivers->refSerial.releaseTransmissionSemaphore(sizeof(GRAPHIC)); } RF_END(); } @@ -433,7 +433,8 @@ modm::ResumableResult RefSerialTransmitter::sendRobotToRobotMsg( reinterpret_cast(robotToRobotMsg), FULL_MSG_SIZE_LESS_MSGLEN + msgLen + sizeof(uint16_t)); - drivers->refSerial.releaseTransmissionSemaphore(); + drivers->refSerial.releaseTransmissionSemaphore( + FULL_MSG_SIZE_LESS_MSGLEN + msgLen + sizeof(uint16_t)); RF_END(); } diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp index f6e4b9f7..4995dcb4 100644 --- a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp +++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp @@ -57,13 +57,13 @@ class DjiMotorEncoder : public tap::encoder::WrappedEncoder // 0 - 8191 for dji motors static constexpr uint16_t ENC_RESOLUTION = 8192; - // Internal gear ratio of the following motors. Output / Input - static constexpr float GEAR_RATIO_M3508 = 187.0f / 3591.0f; - static constexpr float GEAR_RATIO_GM3510_L1 = 1.0f / 3.7f; - static constexpr float GEAR_RATIO_GM3510_L2 = 1.0f / 5.2f; - static constexpr float GEAR_RATIO_GM3510_L3 = 1.0f / 19.0f; - static constexpr float GEAR_RATIO_GM3510_L4 = 1.0f / 27.0f; - static constexpr float GEAR_RATIO_M2006 = 1.0f / 36.0f; + // Internal gear ratio of the following motors + static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f; + static constexpr float GEAR_RATIO_GM3510_L1 = 3.7f / 1.0f; + static constexpr float GEAR_RATIO_GM3510_L2 = 5.2f / 1.0f; + static constexpr float GEAR_RATIO_GM3510_L3 = 19.0f / 1.0f; + static constexpr float GEAR_RATIO_GM3510_L4 = 27.0f / 1.0f; + static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f; static constexpr float GEAR_RATIO_GM6020 = 1.0f / 1.0f; /** diff --git a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp index 60cf4016..a13f98ab 100644 --- a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp @@ -42,7 +42,6 @@ class Odometry2DInterfaceMock : public algorithms::odometry::Odometry2DInterface overrideOdometryPosition, (const float positionX, const float positionY), (override)); - MOCK_METHOD(void, reset, (), (override)); }; } // namespace tap::mock diff --git a/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp b/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp index 58b76f37..d0b595d5 100644 --- a/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp +++ b/ut-robomaster/taproot/test/tap/mock/ref_serial_mock.hpp @@ -54,7 +54,7 @@ class RefSerialMock : public tap::communication::serial::RefSerial (override)); MOCK_METHOD(RobotId, getRobotIdBasedOnCurrentRobotTeam, (RobotId), (override)); MOCK_METHOD(bool, acquireTransmissionSemaphore, (), (override)); - MOCK_METHOD(void, releaseTransmissionSemaphore, (), (override)); + MOCK_METHOD(void, releaseTransmissionSemaphore, (uint32_t), (override)); }; // class RefSerialMock } // namespace mock } // namespace tap From 7a0dc121de7b091be7bf26bd0f07df2e593677bf Mon Sep 17 00:00:00 2001 From: suhas Date: Tue, 17 Jun 2025 02:11:58 -0500 Subject: [PATCH 12/19] update to taproot 2.0 --- ut-robomaster/src/main.cpp | 2 +- .../src/robots/sentry/sentry_control.hpp | 2 +- .../odometry/observer_displacement.cpp | 6 +- .../odometry/observer_displacement.hpp | 1 + .../subsystems/turret/double_yaw_motor.cpp | 8 +-- .../src/subsystems/turret/turret_motor.cpp | 11 +-- .../subsystems/turret/turret_subsystem.cpp | 69 ++++++++++--------- .../src/utils/motors/motor_controller.cpp | 6 +- 8 files changed, 57 insertions(+), 48 deletions(-) diff --git a/ut-robomaster/src/main.cpp b/ut-robomaster/src/main.cpp index a3e8f9b8..b48046cf 100644 --- a/ut-robomaster/src/main.cpp +++ b/ut-robomaster/src/main.cpp @@ -25,7 +25,7 @@ static void initializeIo(src::Drivers *drivers) drivers->schedulerTerminalHandler.init(); drivers->djiMotorTerminalSerialHandler.init(); drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI); - drivers->bmi088.requestRecalibration(); + drivers->bmi088.requestCalibration(); } // Anything that you would like to be called place here. It will be called diff --git a/ut-robomaster/src/robots/sentry/sentry_control.hpp b/ut-robomaster/src/robots/sentry/sentry_control.hpp index f521071c..c90c1f91 100644 --- a/ut-robomaster/src/robots/sentry/sentry_control.hpp +++ b/ut-robomaster/src/robots/sentry/sentry_control.hpp @@ -34,7 +34,7 @@ class SentryControl : CommonControlManual CommandAgitatorContinuous rotateAgitatorL{drivers, &agitatorL, BarrelId::STANDARD1}; CommandAgitatorContinuous rotateAgitatorR{drivers, &agitatorR, BarrelId::STANDARD2}; - CommandSentryPosition sentryPosition{drivers, &chassis}; + CommandSentryPosition sentryPosition{drivers, &chassis, &turret}; CommandSentryAim sentryAim{drivers, &turret}; // Mappings diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp index c08528fd..d49ad64a 100644 --- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp +++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp @@ -29,8 +29,10 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement( // TODO: Depending on when this subsystem gets initialized, // the first time this function runs, deltaT might be large - auto nowTime = imu->getPrevIMUDataReceivedTime(); // Units of us - auto dt = (nowTime - lastTime) / 1e6f; // Want units of s + // auto nowTime = tap::arch::clock::getTimeMicroseconds(); // units of us + auto nowTime = tap::arch::clock::getTimeMilliseconds(); // ms + auto dt = (nowTime - lastTime) / 1000.0f; // sec + // auto dt = (nowTime - lastTime) / 1e6f; // Want units of s // z is 0 since we're moving on the x-y plane and gravity affects z Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f}; diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp index 42626172..379801fb 100644 --- a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp +++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp @@ -2,6 +2,7 @@ #define SUBSYSTEMS_ODOMETRY_OBSERVER_DISPLACEMENT_HPP_ #include "tap/algorithms/odometry/chassis_displacement_observer_interface.hpp" +#include "tap/architecture/clock.hpp" #include "subsystems/chassis/chassis_subsystem.hpp" diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp index b4689c2e..3d7b7d42 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp @@ -34,8 +34,8 @@ void DoubleYawMotor::reset() void DoubleYawMotor::updateMotorAngle() { - float encoderAngle = static_cast(motor1.getEncoderUnwrapped()) / - DjiMotor::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; + float encoderAngle = static_cast(motor1.getEncoder()->getPosition().getWrappedValue()) / + DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; currentAngle.setWrappedValue(encoderAngle); } @@ -64,8 +64,8 @@ void DoubleYawMotor::setVelocity(float velocity, float dt) float DoubleYawMotor::getCurrentVelocity() { - float rpm1 = motor1.getShaftRPM(); // rev / m - float rpm2 = motor2.getShaftRPM(); // rev / m + float rpm1 = motor1.getInternalEncoder().getShaftRPM(); // rev / m + float rpm2 = motor2.getInternalEncoder().getShaftRPM(); // rev / m float currentVelocity = (rpm1 + rpm2) / 2.0f / 60.0f / M3508.gearRatio; // rev / s return currentVelocity; } diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index 6d5c74a8..d8df25fc 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -27,13 +27,14 @@ void TurretMotor::reset() void TurretMotor::updateMotorAngle() { - uint16_t encoderValue = motor.getEncoderWrapped(); + // uint16_t encoderValue = motor.getEncoderWrapped(); + uint16_t encoderValue = motor.getEncoder()->getPosition().getWrappedValue(); if (lastUpdatedEncoderValue != encoderValue) { lastUpdatedEncoderValue = encoderValue; unwrappedAngle = static_cast(encoderValue) * M_TWOPI / - static_cast(DjiMotor::ENC_RESOLUTION); + static_cast(DjiMotorEncoder::ENC_RESOLUTION); currentAngle.setWrappedValue(unwrappedAngle); } } @@ -44,8 +45,10 @@ void TurretMotor::setAngle(float desiredAngle, float dt) float positionControllerError = WrappedFloat(currentAngle.getWrappedValue(), 0, M_TWOPI) .minDifference(setpoint.getWrappedValue()); - float output = - pid.runController(positionControllerError, (M_TWOPI / 60.0f) * motor.getShaftRPM(), dt); + float output = pid.runController( + positionControllerError, + (M_TWOPI / 60.0f) * motor.getInternalEncoder().getShaftRPM(), + dt); motor.setDesiredOutput(output); } diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 3e5e1b18..6bb34754 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -37,45 +37,48 @@ void TurretSubsystem::refresh() // who needs hardware checks? // setAmputated(!hardwareOk()); +#if defined(TARGET_HERO) Remote* remote = &drivers->remote; float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); yaw.setOutput(h); pitch.motor.setDesiredOutput(GM6020.maxOutput * v); + return; +#endif + + yaw.updateMotorAngle(); + pitch.updateMotorAngle(); + +#if defined(TARGET_STANDARD) || defined(TARGET_HERO) + yawEncoder.update(); + + if (!isCalibrated && yawEncoder.isOnline()) + { + baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); + isCalibrated = true; + + setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + } +#else + if (!isCalibrated && !isAmputated()) + { + baseYaw = -YAW_OFFSET; + isCalibrated = true; + + setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); + } +#endif - // yaw.updateMotorAngle(); - // pitch.updateMotorAngle(); - - // #if defined(TARGET_STANDARD) || defined(TARGET_HERO) - // yawEncoder.update(); - - // if (!isCalibrated && yawEncoder.isOnline()) - // { - // baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); - // isCalibrated = true; - - // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - // } - // #else - // if (!isCalibrated && !isAmputated()) - // { - // baseYaw = -YAW_OFFSET; - // isCalibrated = true; - - // setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); - // } - // #endif - - // if (isCalibrated && !drivers->isKillSwitched()) - // { - // yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); - // pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); - // } - // else - // { - // yaw.reset(); - // pitch.reset(); - // } + if (isCalibrated && !drivers->isKillSwitched()) + { + yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); + pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); + } + else + { + yaw.reset(); + pitch.reset(); + } } void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration) diff --git a/ut-robomaster/src/utils/motors/motor_controller.cpp b/ut-robomaster/src/utils/motors/motor_controller.cpp index f0cc69ba..c56be620 100644 --- a/ut-robomaster/src/utils/motors/motor_controller.cpp +++ b/ut-robomaster/src/utils/motors/motor_controller.cpp @@ -50,15 +50,15 @@ bool MotorController::isOnline() { return motor.isMotorOnline(); } float MotorController::measurePosition() { - int64_t encoderVal = motor.getEncoderUnwrapped(); - float units = static_cast(encoderVal) / DjiMotor::ENC_RESOLUTION; + int64_t encoderVal = motor.getEncoder()->getPosition().getUnwrappedValue(); + float units = static_cast(encoderVal) / DjiMotorEncoder::ENC_RESOLUTION; float turns = units / constants.gearRatio; // revs return turns; } float MotorController::measureVelocity() { - int16_t rpm = motor.getShaftRPM() / constants.gearRatio; + int16_t rpm = motor.getInternalEncoder().getShaftRPM() / constants.gearRatio; float rps = rpm / 60.0f; // revs / sec return rps; } From 584970510bddb835c3fd2e5c147bdb87da4bb86c Mon Sep 17 00:00:00 2001 From: suhas Date: Sun, 22 Jun 2025 10:29:18 -0500 Subject: [PATCH 13/19] make sentry just spin --- .../chassis/command_sentry_position.cpp | 82 +++++++++---------- 1 file changed, 39 insertions(+), 43 deletions(-) diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index 812472e7..b2ba7827 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -10,51 +10,51 @@ void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } void CommandSentryPosition::execute() { - float yawAngle = turret->getTargetLocalYaw(); - Vector2f inputMove = Vector2f(0.0f); - float inputSpin = 0.0f; + // float yawAngle = turret->getTargetLocalYaw(); + // Vector2f inputMove = Vector2f(0.0f); + // float inputSpin = 0.0f; - // get keyboard input - float keyboardInputSpin; - bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); + // // get keyboard input + // float keyboardInputSpin; + // bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); - // get joystick input - Vector2f joystickInputMove; - float joystickInputSpin; - bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin); + // // get joystick input + // Vector2f joystickInputMove; + // float joystickInputSpin; + // bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin); - // decide which input source to use (keyboard has inertia) - if (!hasKeyboardInput && hasJoystickInput) - { - inputMove = joystickInputMove; - inputSpin = joystickInputSpin; - } - else - { - inputMove = keyboardInputMove; - inputSpin = keyboardInputSpin; - } + // // decide which input source to use (keyboard has inertia) + // if (!hasKeyboardInput && hasJoystickInput) + // { + // inputMove = joystickInputMove; + // inputSpin = joystickInputSpin; + // } + // else + // { + // inputMove = keyboardInputMove; + // inputSpin = keyboardInputSpin; + // } - // auto-align chassis to turret when moving - if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) - { - inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * - CHASSIS_AUTOALIGN_FACTOR; - } + // // auto-align chassis to turret when moving + // if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) + // { + // inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * + // CHASSIS_AUTOALIGN_FACTOR; + // } - // override spin input while beyblading - if (beyblade) - { - inputSpin = 1.0f; - } + // // override spin input while beyblading + // if (beyblade) + // { + // inputSpin = 1.0f; + // } - // rotate movement vector relative to turret - if (turretRelative) - { - inputMove = inputMove.rotate(yawAngle); - } + // // rotate movement vector relative to turret + // if (turretRelative) + // { + // inputMove = inputMove.rotate(yawAngle); + // } - chassis->input(inputMove, inputSpin); + // chassis->input(inputMove, inputSpin); // if (drivers->isGameActive() && moveTimer.isStopped()) // { @@ -68,14 +68,13 @@ void CommandSentryPosition::execute() // } // // speen - // chassis->input(Vector2f(0.0f), 1.0f); + chassis->input(Vector2f(0.0f), 1.0f); } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } bool CommandSentryPosition::isFinished() const { return false; } - bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &inputSpin) { Remote *remote = &drivers->remote; @@ -107,8 +106,6 @@ bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &input return rawMoveInput != Vector2f(0.0f); } - - bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &inputSpin) { Remote *remote = &drivers->remote; @@ -141,5 +138,4 @@ bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &input return inputMove != Vector2f(0.0f) || inputSpin != 0.0f; } - } // namespace commands From 4bad9ec3ae1cc0b7428973c9b5cf6c65dd94d4ed Mon Sep 17 00:00:00 2001 From: suhas Date: Sun, 22 Jun 2025 21:54:57 -0700 Subject: [PATCH 14/19] finish --- ut-robomaster/src/communication/cv_board.cpp | 16 +++++++++- ut-robomaster/src/communication/cv_board.hpp | 19 +++++++++++- .../src/communication/cv_message.hpp | 8 +++++ .../chassis/command_sentry_position.cpp | 29 +++++++++++-------- .../chassis/command_sentry_position.hpp | 11 ++++--- .../subsystems/turret/command_sentry_aim.cpp | 21 ++++++++++++++ 6 files changed, 84 insertions(+), 20 deletions(-) diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index 4de325cd..584b1b51 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -26,6 +26,9 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message) break; // case CV_MESSAGE_TYPE_ECHO: // echoData(message); + case CV_MESSAGE_TYPE_POSITION_REQUEST: + decodePositionRequest(message); + break; default: break; } @@ -37,7 +40,7 @@ void CVBoard::sendMessage() sendColorData(); } -// void CVBoard::echoData(const ReceivedSerialMessage& message) +// void CVBoard::echoData(const ReceivedSerialMessage& message) // { // std::string data_string; // memcpy(&data_string, &message.data, sizeof(message.header.dataLength)); @@ -95,6 +98,17 @@ bool CVBoard::decodeTurretData(const ReceivedSerialMessage& message) return false; } +bool CVBoard::decodePositionRequest(const ReceivedSerialMessage& message) +{ + if (message.header.dataLength == sizeof(lastPositionRequest)) + { + memcpy(&lastPositionRequest, &message.data, sizeof(lastPositionRequest)); + return true; + } + return false; +} + bool CVBoard::isOnline() const { return !offlineTimeout.isExpired(); } const TurretData& CVBoard::getTurretData() const { return lastTurretData; } +const PositionRequest& CVBoard::getPositionRequest() const { return lastPositionRequest; } } // namespace communication diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp index c6123f7f..9949cade 100644 --- a/ut-robomaster/src/communication/cv_board.hpp +++ b/ut-robomaster/src/communication/cv_board.hpp @@ -46,7 +46,7 @@ class CVBoard : public tap::communication::serial::DJISerial /** * Echo the message sent by the CV board to the CV board */ - // void echoData(const ReceivedSerialMessage& message); + void echoData(const ReceivedSerialMessage& message); /** * Sends odometry data to the CV board @@ -66,6 +66,18 @@ class CVBoard : public tap::communication::serial::DJISerial */ bool decodeTurretData(const ReceivedSerialMessage& message); + /** + * Decode a request to move to a position. + * @param message message from CV board + * @return true if valid, false otherwise + */ + bool decodePositionRequest(const ReceivedSerialMessage& message); + + /** + * Fetch the position request from the CV board + */ + const PositionRequest& getPositionRequest() const; + /** * @return true if a message has been received within the last OFFLINE_TIMEOUT_MS milliseconds, * false if otherwise @@ -89,6 +101,11 @@ class CVBoard : public tap::communication::serial::DJISerial /** Last turret aiming data received from the CV board */ TurretData lastTurretData; + PositionRequest lastPositionRequest = { + .x_requested = -999.9f, + .y_requested = -999.9f, + .time_sent = 0xdeadbeef}; + /** UART port used to communicate with the CV board */ static constexpr Uart::UartPort UART_PORT = Uart::Uart1; diff --git a/ut-robomaster/src/communication/cv_message.hpp b/ut-robomaster/src/communication/cv_message.hpp index 5899757a..146f406c 100644 --- a/ut-robomaster/src/communication/cv_message.hpp +++ b/ut-robomaster/src/communication/cv_message.hpp @@ -12,6 +12,7 @@ enum MessageTypes : uint8_t CV_MESSAGE_TYPE_TURRET_AIM = 2, CV_MESSAGE_TYPE_COLOR_DATA = 3, CV_MESSAGE_TYPE_ECHO = 4, + CV_MESSAGE_TYPE_POSITION_REQUEST = 5, }; enum ColorTypes : uint8_t @@ -38,6 +39,13 @@ struct TurretData bool hasTarget; } modm_packed; +struct PositionRequest +{ + float x_requested; + float y_requested; + uint64_t time_sent; +} modm_packed; + struct OdometryData { float xPos; diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index b2ba7827..da1dc92c 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -3,10 +3,11 @@ namespace commands { +using namespace communication; // old ard code -// void CommandSentryPosition::initialize() { moveTimer.stop(); } +void CommandSentryPosition::initialize() { moveTimer.stop(); } -void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } +// void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } void CommandSentryPosition::execute() { @@ -56,18 +57,22 @@ void CommandSentryPosition::execute() // chassis->input(inputMove, inputSpin); - // if (drivers->isGameActive() && moveTimer.isStopped()) - // { - // moveTimer.restart(10'000); // 10s - // } + if (drivers->isGameActive() && moveTimer.isStopped()) + { + moveTimer.restart(5'000); // 5 secs + } - // if (!moveTimer.isExpired()) - // { - // chassis->input(Vector2f(0.0f), 0.0f); - // return; - // } + if (!moveTimer.isExpired()) + { + chassis->input(Vector2f(0.0f), 1.0f); // spin! + return; + } + + PositionRequest pos = drivers->cvBoard.getPositionRequest(); + drivers->rtt << "x_pos = " << pos.x_requested << ", y_pos = " << pos.y_requested + << ", time_sent = " << (float)pos.time_sent; - // // speen + // speen chassis->input(Vector2f(0.0f), 1.0f); } diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp index 0067a949..546fc89c 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp @@ -1,12 +1,11 @@ #pragma once -//#include "tap/architecture/timeout.hpp" +// #include "tap/architecture/timeout.hpp" #include "tap/control/command.hpp" - #include "robots/robot_constants.hpp" -//this is older sentry code dont know why it had chopped path -//#include "chassis_subsystem.hpp" +// this is older sentry code dont know why it had chopped path +// #include "chassis_subsystem.hpp" #include "subsystems/chassis/chassis_subsystem.hpp" #include "subsystems/turret/turret_subsystem.hpp" #include "utils/chassis_auto_align.hpp" @@ -18,8 +17,8 @@ namespace commands using namespace tap::communication::serial; using namespace modm; using subsystems::chassis::ChassisSubsystem; -// using tap::arch::MilliTimeout; using subsystems::turret::TurretSubsystem; +using tap::arch::MilliTimeout; class CommandSentryPosition : public tap::control::Command { @@ -52,7 +51,7 @@ class CommandSentryPosition : public tap::control::Command private: src::Drivers *drivers; ChassisSubsystem *chassis; - //MilliTimeout moveTimer; + MilliTimeout moveTimer; TurretSubsystem *turret; Vector2f keyboardInputMove = Vector2f(0.0f); diff --git a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp index 895c9b91..656942bc 100644 --- a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp +++ b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp @@ -7,6 +7,27 @@ void CommandSentryAim::initialize() {} void CommandSentryAim::execute() { // turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f); + if (turret->getTargetLocalYaw() <= -55.0f) + { + turret->setTargetWorldAngles(50.0f, 10.0f); + } + else + { + turret->setTargetWorldAngles(-50.0f, -10.0f); + } + + /* + PLAN (for suhas): + - see if setTargetWorldAngles() works as intended + - create targetWorldYaw/pitch variables + - twYaw should switch between -200 and 200 depending on what its set to + - if the angle is -200 set it to 200 + - if the angle is 200 set it to -200 + - else, keep current targetWorldAngle + - time how long it takes for pitch motor to oscillate + - use a timer to oscillate pitch to move ~10 deg to scan, or if that doesn't work, use the + - same logic to scan + */ } void CommandSentryAim::end(bool) {} From 8c02aad1cce8db356227676ad51d410b11cc8e15 Mon Sep 17 00:00:00 2001 From: Suhas Date: Tue, 24 Jun 2025 23:34:08 +0000 Subject: [PATCH 15/19] dtyuidrthgfuhfjg --- .../src/robots/hero/hero_constants.hpp | 2 +- .../subsystems/turret/double_yaw_motor.cpp | 9 +++++---- .../src/subsystems/turret/turret_motor.cpp | 2 +- .../subsystems/turret/turret_subsystem.cpp | 20 +++++-------------- .../src/utils/motors/motor_controller.cpp | 2 +- 5 files changed, 13 insertions(+), 22 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 05162afd..dc689351 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -44,7 +44,7 @@ constexpr float CAMERA_X_OFFSET = 0.0f; // horizontal offset of main camera l constexpr float PITCH_MIN = -0.3349f; // rad constexpr float PITCH_MAX = 0.3534f; // rad -static constexpr float YAW_REDUCTION = 1.0f; +static constexpr float YAW_REDUCTION = 2.005459491f; static constexpr float PITCH_REDUCTION = 1.0f; // Tuning Constants ----------------------------------- diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp index 3d7b7d42..11f3609d 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp @@ -34,8 +34,9 @@ void DoubleYawMotor::reset() void DoubleYawMotor::updateMotorAngle() { - float encoderAngle = static_cast(motor1.getEncoder()->getPosition().getWrappedValue()) / - DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; + float encoderAngle = + static_cast(motor1.getInternalEncoder().getEncoder().getUnwrappedValue()) / + DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; currentAngle.setWrappedValue(encoderAngle); } @@ -47,8 +48,8 @@ void DoubleYawMotor::setAngle(float desiredAngle, float dt) .minDifference(setpoint.getWrappedValue()); // account for chassis rotation - float disturbance = drivers->bmi088.getGz() / 360.0f; // rev / s - disturbance *= 2.0f; // seems to help? + float disturbance = drivers->bmi088.getGz() / 2.0f / PI; // rev / s + disturbance *= 2.0f; // seems to help? float targetVelocity = positionPid.update(positionError, dt, false) - disturbance; setVelocity(targetVelocity, dt); diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index d8df25fc..fe8c8432 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -28,7 +28,7 @@ void TurretMotor::reset() void TurretMotor::updateMotorAngle() { // uint16_t encoderValue = motor.getEncoderWrapped(); - uint16_t encoderValue = motor.getEncoder()->getPosition().getWrappedValue(); + uint16_t encoderValue = motor.getInternalEncoder().getEncoder().getUnwrappedValue(); if (lastUpdatedEncoderValue != encoderValue) { lastUpdatedEncoderValue = encoderValue; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 6bb34754..0d47f2a8 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -32,20 +32,6 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { - // yaw.setOutput(0.5f); - // pitch.motor.setDesiredOutput(M3508.maxOutput * 0.5); - // who needs hardware checks? - // setAmputated(!hardwareOk()); - -#if defined(TARGET_HERO) - Remote* remote = &drivers->remote; - float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL); - float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL); - yaw.setOutput(h); - pitch.motor.setDesiredOutput(GM6020.maxOutput * v); - return; -#endif - yaw.updateMotorAngle(); pitch.updateMotorAngle(); @@ -54,7 +40,11 @@ void TurretSubsystem::refresh() if (!isCalibrated && yawEncoder.isOnline()) { +#if defined(TARGET_HERO) + baseYaw = -yaw.getAngle(); +#else baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle(); +#endif isCalibrated = true; setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); @@ -94,7 +84,7 @@ void TurretSubsystem::setTargetWorldAngles(float yaw, float pitch) targetWorldPitch = modm::min(modm::max(pitch, PITCH_MIN), PITCH_MAX); } -float TurretSubsystem::getChassisYaw() { return modm::toRadian(drivers->bmi088.getYaw()); } +float TurretSubsystem::getChassisYaw() { return drivers->bmi088.getYaw(); } float TurretSubsystem::getTargetLocalYaw() { return targetWorldYaw - getChassisYaw(); } diff --git a/ut-robomaster/src/utils/motors/motor_controller.cpp b/ut-robomaster/src/utils/motors/motor_controller.cpp index c56be620..74723ebd 100644 --- a/ut-robomaster/src/utils/motors/motor_controller.cpp +++ b/ut-robomaster/src/utils/motors/motor_controller.cpp @@ -50,7 +50,7 @@ bool MotorController::isOnline() { return motor.isMotorOnline(); } float MotorController::measurePosition() { - int64_t encoderVal = motor.getEncoder()->getPosition().getUnwrappedValue(); + int64_t encoderVal = motor.getInternalEncoder().getEncoder().getUnwrappedValue(); float units = static_cast(encoderVal) / DjiMotorEncoder::ENC_RESOLUTION; float turns = units / constants.gearRatio; // revs return turns; From 8ecec064f3797c73703d81bffa1ac2b344b14726 Mon Sep 17 00:00:00 2001 From: Suhas Date: Wed, 25 Jun 2025 04:00:26 +0000 Subject: [PATCH 16/19] patch taproot!!!! --- .../taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp index 5e70867f..eb038528 100644 --- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp @@ -121,7 +121,7 @@ class Bmi088 final_mockable : public Bmi088Data, public AbstractIMU private: Drivers *drivers; - inline float getAccelerationSensitivity() const override { return ACC_G_PER_ACC_COUNT; } + inline float getAccelerationSensitivity() const override { return tap::algorithms::ACCELERATION_GRAVITY / ACC_G_PER_ACC_COUNT; } static constexpr uint16_t RAW_TEMPERATURE_TO_APPLY_OFFSET = 1023; /// Offset parsed temperature reading by this amount if > RAW_TEMPERATURE_TO_APPLY_OFFSET. From 177258e194040b21face409e99da981bd8632c93 Mon Sep 17 00:00:00 2001 From: Suhas Date: Wed, 25 Jun 2025 10:13:31 +0000 Subject: [PATCH 17/19] the good stuff --- ut-robomaster/src/robots/hero/hero_constants.hpp | 10 +++++----- .../src/robots/standard/standard_constants.hpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index dc689351..378b8d0b 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -41,8 +41,10 @@ constexpr float CAMERA_TO_PITCH = 0.0f; // distance from main camera lens to constexpr float NOZZLE_TO_PITCH = 0.0f; // distance from barrel nozzle to pitch axis (m) constexpr float CAMERA_TO_BARRELS = 0.0f; // vertical ctc offset from camera lens to barrel (m) constexpr float CAMERA_X_OFFSET = 0.0f; // horizontal offset of main camera lens (m) -constexpr float PITCH_MIN = -0.3349f; // rad -constexpr float PITCH_MAX = 0.3534f; // rad +constexpr float PITCH_MIN = 0.15f; // rad +constexpr float PITCH_MAX = 0.60f; // rad +const float YAW_OFFSET = 0; // deprecated on hero until gear ratios are fixed +const float PITCH_OFFSET = 1.90f; static constexpr float YAW_REDUCTION = 2.005459491f; static constexpr float PITCH_REDUCTION = 1.0f; @@ -126,8 +128,6 @@ const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; -const float YAW_OFFSET = 0; -const float PITCH_OFFSET = 0; // Velocities ---------------------------- @@ -141,7 +141,7 @@ static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s const float TARGET_PROJECTILE_VELOCITY = 16; // m/s -const float FLYWHEEL_SPEED = 47.5f; +const float FLYWHEEL_SPEED = 190.0f; const float BALLS_PER_SEC = 4.0f; const float BALLS_PER_REV = 6.0f; diff --git a/ut-robomaster/src/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp index 86f44722..993594ff 100644 --- a/ut-robomaster/src/robots/standard/standard_constants.hpp +++ b/ut-robomaster/src/robots/standard/standard_constants.hpp @@ -42,7 +42,7 @@ constexpr float CAMERA_TO_BARRELS = 0.0427f; // vertical ctc offset from camera constexpr float CAMERA_X_OFFSET = -0.0335f; // horizontal offset of main camera lens (m) // static constexpr float YAW_OFFSET = 0.0293f; // external encoder + motor encoder angles -static constexpr float YAW_OFFSET = 0.0f; +static constexpr float YAW_OFFSET = -2.3f; static constexpr float PITCH_OFFSET = 2.79f; static constexpr float YAW_REDUCTION = 2.0f; static constexpr float PITCH_REDUCTION = 1.0f; From 4fd5bb13c07806807637bf46e95eaa8ead101644 Mon Sep 17 00:00:00 2001 From: Suhas Date: Wed, 25 Jun 2025 10:51:42 +0000 Subject: [PATCH 18/19] fix GM6020 current control and add current control support (dm arthur) --- .../src/robots/hero/hero_constants.hpp | 61 ++++++++++++++----- .../src/robots/sentry/sentry_constants.hpp | 43 +++++++++---- .../robots/standard/standard_constants.hpp | 51 ++++++++++++---- .../src/subsystems/turret/turret_motor.cpp | 8 ++- .../subsystems/turret/turret_subsystem.cpp | 7 +++ .../src/utils/motors/motor_constants.hpp | 1 + .../src/utils/motors/motor_controller.hpp | 2 +- 7 files changed, 131 insertions(+), 42 deletions(-) diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 378b8d0b..05fcce9d 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -107,27 +107,60 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1; constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis -const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; - -// flywheels const MotorConfig - FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, false, "flywheel left", PID_FLYWHEEL, {}}; + WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false}; const MotorConfig - FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}}; + WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false}; + +// flywheels +const MotorConfig FLYWHEEL_L{ + M3508_NOGEARBOX, + MOTOR3, + CAN_SHOOTER, + false, + "flywheel left", + PID_FLYWHEEL, + {}, + false}; +const MotorConfig FLYWHEEL_R{ + M3508_NOGEARBOX, + MOTOR4, + CAN_SHOOTER, + true, + "flywheel right", + PID_FLYWHEEL, + {}, + false}; // agitator -const MotorConfig - AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; // water wheel -const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}}; +const MotorConfig AGITATOR{ + M3508, + MOTOR1, + CAN_SHOOTER, + false, + "agitator", + PID_AGITATOR, + {}, + false}; // water wheel +const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}, false}; // turret +const MotorConfig YAW_L{ + M3508, + MOTOR5, + CAN_TURRET, + false, + "yaw left", + {2.5f, 60.0f, 0.0f}, + {45.0f, 0.0f, 0.0f}, + false}; +const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}, false}; const MotorConfig - YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; -const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; + PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false}; // Velocities ---------------------------- diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp index 967a8a5e..f9e272ed 100644 --- a/ut-robomaster/src/robots/sentry/sentry_constants.hpp +++ b/ut-robomaster/src/robots/sentry/sentry_constants.hpp @@ -100,14 +100,25 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1; constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis -const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; +const MotorConfig + WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false}; // flywheels -const MotorConfig - FLYWHEEL_TL{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel top left", PID_FLYWHEEL, {}}; +const MotorConfig FLYWHEEL_TL{ + M3508_NOGEARBOX, + MOTOR3, + CAN_SHOOTER, + true, + "flywheel top left", + PID_FLYWHEEL, + {}, + false}; const MotorConfig FLYWHEEL_TR{ M3508_NOGEARBOX, MOTOR4, @@ -115,7 +126,8 @@ const MotorConfig FLYWHEEL_TR{ false, "flywheel top right", PID_FLYWHEEL, - {}}; + {}, + false}; const MotorConfig FLYWHEEL_BL{ M3508_NOGEARBOX, MOTOR5, @@ -123,7 +135,8 @@ const MotorConfig FLYWHEEL_BL{ false, "flywheel bottom left", PID_FLYWHEEL, - {}}; + {}, + false}; const MotorConfig FLYWHEEL_BR{ M3508_NOGEARBOX, MOTOR6, @@ -131,15 +144,19 @@ const MotorConfig FLYWHEEL_BR{ true, "flywheel bottom right", PID_FLYWHEEL, - {}}; + {}, + false}; // agitator -const MotorConfig AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR, {}}; -const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR, {}}; +const MotorConfig + AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR, {}, false}; +const MotorConfig + AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR, {}, false}; // turret -const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}}; -const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}, false}; +const MotorConfig + PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false}; const float YAW_OFFSET = 0; const float PITCH_OFFSET = 0; diff --git a/ut-robomaster/src/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp index 993594ff..28de97b5 100644 --- a/ut-robomaster/src/robots/standard/standard_constants.hpp +++ b/ut-robomaster/src/robots/standard/standard_constants.hpp @@ -105,27 +105,52 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1; constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2; // chassis -const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}}; -const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}}; - -// flywheels const MotorConfig - FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL, {}}; + WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false}; +const MotorConfig + WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false}; const MotorConfig - FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL, {}}; + WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false}; + +// flywheels +const MotorConfig FLYWHEEL_L{ + M3508_NOGEARBOX, + MOTOR3, + CAN_SHOOTER, + true, + "flywheel left", + PID_FLYWHEEL, + {}, + false}; +const MotorConfig FLYWHEEL_R{ + M3508_NOGEARBOX, + MOTOR4, + CAN_SHOOTER, + false, + "flywheel right", + PID_FLYWHEEL, + {}, + false}; // agitator -const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}}; +const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}, false}; // turret // {30.0f, 0.0f, 0.0f} // {60.0f, 0.0f, 1.0f} -const MotorConfig - YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}}; -const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}}; -const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}}; +const MotorConfig YAW_L{ + M3508, + MOTOR5, + CAN_TURRET, + false, + "yaw left", + {2.5f, 60.0f, 0.0f}, + {45.0f, 0.0f, 0.0f}, + false}; +const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}, false}; +const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, true}; // Velocities ------------------------------------- diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp index fe8c8432..e6c075e1 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -9,7 +9,13 @@ TurretMotor::TurretMotor( src::Drivers *drivers, MotorConfig motorConfig, const SmoothPidConfig &pidConfig) - : motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name), + : motor( + drivers, + motorConfig.id, + motorConfig.canBus, + motorConfig.inverted, + motorConfig.name, + motorConfig.currentControl), drivers(drivers), pid(pidConfig), setpoint(0.0f, 0.0f, M_TWOPI), diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 0d47f2a8..1e1f6d4e 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -35,6 +35,9 @@ void TurretSubsystem::refresh() yaw.updateMotorAngle(); pitch.updateMotorAngle(); + drivers->rtt << "pitch: " << pitch.getAngle(); + drivers->rtt.endl(); + #if defined(TARGET_STANDARD) || defined(TARGET_HERO) yawEncoder.update(); @@ -62,7 +65,11 @@ void TurretSubsystem::refresh() if (isCalibrated && !drivers->isKillSwitched()) { yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT); + // pitch.motor.setDesiredOutput(GM6020.maxOutput - 1); pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT); + drivers->rtt << "target pitch angle " + << (PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION; + drivers->rtt.endl(); } else { diff --git a/ut-robomaster/src/utils/motors/motor_constants.hpp b/ut-robomaster/src/utils/motors/motor_constants.hpp index d99c6eef..0313c84f 100644 --- a/ut-robomaster/src/utils/motors/motor_constants.hpp +++ b/ut-robomaster/src/utils/motors/motor_constants.hpp @@ -24,6 +24,7 @@ struct MotorConfig const char* const name; PidConstants velocityPidConstants; PidConstants positionPidConstants; + bool currentControl; }; // C620 controller (16384 = 20A) diff --git a/ut-robomaster/src/utils/motors/motor_controller.hpp b/ut-robomaster/src/utils/motors/motor_controller.hpp index 79603dd5..5303c2fb 100644 --- a/ut-robomaster/src/utils/motors/motor_controller.hpp +++ b/ut-robomaster/src/utils/motors/motor_controller.hpp @@ -21,7 +21,7 @@ class MotorController public: MotorController(src::Drivers* drivers, const MotorConfig motor) : constants(motor.constants), - motor(drivers, motor.id, motor.canBus, motor.inverted, motor.name), + motor(drivers, motor.id, motor.canBus, motor.inverted, motor.name, motor.currentControl), velocityPid(motor.velocityPidConstants), positionPid(motor.positionPidConstants) { From b0fb06aba0a66861458e9b1470b80d552cb0a16d Mon Sep 17 00:00:00 2001 From: Suhas Date: Thu, 26 Jun 2025 20:24:58 +0000 Subject: [PATCH 19/19] tuning and heat buffer --- .../src/robots/hero/hero_constants.hpp | 25 +++++++- ut-robomaster/src/robots/robot_constants.hpp | 4 +- .../src/robots/sentry/sentry_constants.hpp | 21 ++++++- .../robots/standard/standard_constants.hpp | 32 +++++++++-- .../src/robots/standard/standard_control.hpp | 8 +++ .../agitator/command_agitator_burst.cpp | 45 ++++++++++++++- .../agitator/command_agitator_continuous.cpp | 57 ++++++++++++++++++- .../chassis/command_move_chassis.cpp | 11 +++- .../chassis/command_sentry_position.cpp | 34 ++++++----- .../turret/command_recalibrate_turret.cpp | 19 +++++++ .../turret/command_recalibrate_turret.hpp | 39 +++++++++++++ .../subsystems/turret/command_sentry_aim.cpp | 15 +++-- .../subsystems/turret/turret_subsystem.cpp | 7 ++- .../subsystems/turret/turret_subsystem.hpp | 3 + 14 files changed, 284 insertions(+), 36 deletions(-) create mode 100644 ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp create mode 100644 ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 05fcce9d..39345cd7 100644 --- a/ut-robomaster/src/robots/hero/hero_constants.hpp +++ b/ut-robomaster/src/robots/hero/hero_constants.hpp @@ -2,6 +2,7 @@ #include "tap/algorithms/smooth_pid.hpp" #include "tap/communication/can/can_bus.hpp" +#include "tap/communication/serial/ref_serial.hpp" #include "tap/motor/dji_motor.hpp" #include "modm/container/pair.hpp" @@ -13,6 +14,7 @@ using motor_controller::PidConstants; using tap::can::CanBus; using namespace tap::motor; using namespace motors; +using tap::communication::serial::RefSerial; // General constants ------------------------------------------------ @@ -97,8 +99,8 @@ static constexpr float YAW_INPUT_SCALE = 4.0f; static constexpr float PITCH_INPUT_SCALE = 5.0f; #endif -static constexpr float MOUSE_SENS_YAW = 0.0045f; -static constexpr float MOUSE_SENS_PITCH = 0.002f; +static constexpr float MOUSE_SENS_YAW = 0.0090f; +static constexpr float MOUSE_SENS_PITCH = 0.0025f; // Motor Constants ------------------------------------- @@ -187,4 +189,21 @@ const float UNJAM_SPEED = 12.0f; // rev/s // Heat Buffers --------------------------- -const uint16_t BARREL_HEAT_BUFFER = 100.0f; +const uint16_t BARREL_HEAT_BUFFER_1V1 = 100; +const uint16_t BARREL_HEAT_BUFFER_3V3[10] = { + 100, // Level 1 + 100, // Level 2 + 100, // Level 3 + 100, // Level 4 + 100, // Level 5 + 100, // Level 6 + 100, // Level 7 + 100, // Level 8 + 100, // Level 9 + 100 // Level 10 +}; + +constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true; +const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE = + RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3; +const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1; \ No newline at end of file diff --git a/ut-robomaster/src/robots/robot_constants.hpp b/ut-robomaster/src/robots/robot_constants.hpp index f3d30450..0f13b737 100644 --- a/ut-robomaster/src/robots/robot_constants.hpp +++ b/ut-robomaster/src/robots/robot_constants.hpp @@ -10,8 +10,8 @@ const uint32_t REFRESH_PERIOD = 2; // ms constexpr float DT = REFRESH_PERIOD / 1000.0f; // refresh delta time (s) const float ANALOG_DEAD_ZONE = 0.1f; -const float KEYBOARD_ACCEL = 5.0f; -const float KEYBOARD_DECEL = 5.0f; +const float KEYBOARD_ACCEL = 3.5f; +const float KEYBOARD_DECEL = 3.5f; const float CHASSIS_AUTOALIGN_FACTOR = 0.5f; const float CHASSIS_AUTOALIGN_ANGLE = M_PI; diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp index f9e272ed..7901875a 100644 --- a/ut-robomaster/src/robots/sentry/sentry_constants.hpp +++ b/ut-robomaster/src/robots/sentry/sentry_constants.hpp @@ -2,6 +2,7 @@ #include "tap/algorithms/smooth_pid.hpp" #include "tap/communication/can/can_bus.hpp" +#include "tap/communication/serial/ref_serial.hpp" #include "tap/motor/dji_motor.hpp" #include "modm/container/pair.hpp" @@ -13,6 +14,7 @@ using motor_controller::PidConstants; using tap::can::CanBus; using namespace tap::motor; using namespace motors; +using tap::communication::serial::RefSerial; // General constants ------------------------------------------------ @@ -184,4 +186,21 @@ const float UNJAM_SPEED = 15.0f; // rev/s // Heat Buffers ------------------------------------- -const uint16_t BARREL_HEAT_BUFFER = 20.0f; +const uint16_t BARREL_HEAT_BUFFER_1V1 = 50; +const uint16_t BARREL_HEAT_BUFFER_3V3[10] = { + 40, // Level 1 + 50, // Level 2 + 50, // Level 3 + 50, // Level 4 + 50, // Level 5 + 50, // Level 6 + 50, // Level 7 + 50, // Level 8 + 50, // Level 9 + 50 // Level 10 +}; + +constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true; +const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE = + RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3; +const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1; \ No newline at end of file diff --git a/ut-robomaster/src/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp index 28de97b5..5a3270ec 100644 --- a/ut-robomaster/src/robots/standard/standard_constants.hpp +++ b/ut-robomaster/src/robots/standard/standard_constants.hpp @@ -2,6 +2,7 @@ #include "tap/algorithms/smooth_pid.hpp" #include "tap/communication/can/can_bus.hpp" +#include "tap/communication/serial/ref_serial.hpp" #include "tap/motor/dji_motor.hpp" #include "modm/container/pair.hpp" @@ -13,6 +14,7 @@ using motor_controller::PidConstants; using tap::can::CanBus; using namespace tap::motor; using namespace motors; +using tap::communication::serial::RefSerial; // General constants ------------------------------------------------ @@ -34,7 +36,7 @@ static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f; static constexpr int FLYWHEELS = 2; // turret ------------ -constexpr float PITCH_MIN = 0.10f; // rad +constexpr float PITCH_MIN = 0.20f; // rad constexpr float PITCH_MAX = 0.79f; // rad constexpr float CAMERA_TO_PITCH = 0.13555f; // distance from main camera lens to pitch axis (m) constexpr float NOZZLE_TO_PITCH = 0.18151f; // distance from barrel nozzle to pitch axis (m) @@ -42,7 +44,8 @@ constexpr float CAMERA_TO_BARRELS = 0.0427f; // vertical ctc offset from camera constexpr float CAMERA_X_OFFSET = -0.0335f; // horizontal offset of main camera lens (m) // static constexpr float YAW_OFFSET = 0.0293f; // external encoder + motor encoder angles -static constexpr float YAW_OFFSET = -2.3f; +static constexpr float YAW_OFFSET = 4.30f; +// static constexpr float YAW_OFFSET = -2.3f; static constexpr float PITCH_OFFSET = 2.79f; static constexpr float YAW_REDUCTION = 2.0f; static constexpr float PITCH_REDUCTION = 1.0f; @@ -95,8 +98,8 @@ static constexpr float YAW_INPUT_SCALE = 10.0f; static constexpr float PITCH_INPUT_SCALE = 5.0f; #endif -static constexpr float MOUSE_SENS_YAW = 0.1f; -static constexpr float MOUSE_SENS_PITCH = 0.1f; +static constexpr float MOUSE_SENS_YAW = 0.05f; +static constexpr float MOUSE_SENS_PITCH = 0.05f; // Motor constants -------------------------------- @@ -169,11 +172,28 @@ const float FLYWHEEL_SPEED = 160.0f; const float BALLS_PER_SEC = 17.0f; const float BALLS_PER_REV = 18.0f; -const float JAM_TRIGGER_RATIO = 0.2; // measured speed to driven speed ratio +const float JAM_TRIGGER_RATIO = 0.2f; // measured speed to driven speed ratio const float JAM_TRIGGER_DURATION = 0.1f; // s const float UNJAM_DURATION = 0.1f; // s const float UNJAM_SPEED = 30.0f; // rev/s // Heat Buffers ------------------------------------- -const uint16_t BARREL_HEAT_BUFFER = 50.0f; +const uint16_t BARREL_HEAT_BUFFER_1V1 = 50; +const uint16_t BARREL_HEAT_BUFFER_3V3[10] = { + 40, // Level 1 + 50, // Level 2 + 50, // Level 3 + 50, // Level 4 + 50, // Level 5 + 50, // Level 6 + 50, // Level 7 + 50, // Level 8 + 50, // Level 9 + 50 // Level 10 +}; + +constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true; +const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE = + RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3; +const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1; \ No newline at end of file diff --git a/ut-robomaster/src/robots/standard/standard_control.hpp b/ut-robomaster/src/robots/standard/standard_control.hpp index d9aa5d20..3a6eb91d 100644 --- a/ut-robomaster/src/robots/standard/standard_control.hpp +++ b/ut-robomaster/src/robots/standard/standard_control.hpp @@ -1,5 +1,6 @@ #include "robots/common/common_control_manual.hpp" #include "subsystems/agitator/command_agitator_continuous.hpp" +#include "subsystems/turret/command_recalibrate_turret.hpp" class StandardControl : CommonControlManual { @@ -29,6 +30,8 @@ class StandardControl : CommonControlManual BarrelId::STANDARD1, true}; + CommandRecalibrateTurret recalibrateTurret_mmb{drivers, &turret}; + // Mappings HoldCommandMapping leftMouseDown{ drivers, @@ -39,4 +42,9 @@ class StandardControl : CommonControlManual drivers, {&rotateAgitator_SwitchUp, &rotateFlywheel_SwitchMid}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)}; + + PressCommandMapping rightMousePress{ + drivers, + {&recalibrateTurret_mmb}, + RemoteMapState(RemoteMapState::MouseButton::RIGHT)}; }; \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp b/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp index 1a68a147..ef9bf454 100644 --- a/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp +++ b/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp @@ -1,5 +1,6 @@ #include "command_agitator_burst.hpp" +using tap::communication::serial::RefSerialData; namespace commands { void CommandAgitatorBurst::initialize() { initialPosition = agitator->getPosition(); } @@ -15,8 +16,50 @@ bool CommandAgitatorBurst::isFinished() const return true; } + uint16_t barrel_heat_buffer = 0.0; + if (drivers->refSerial.getRefSerialReceivingData()) + { + tap::communication::serial::RefSerialData::Rx::GameType game_type = + drivers->refSerial.getGameData().gameType; + uint8_t robot_level = drivers->refSerial.getRobotData().robotLevel; + if (game_type == RefSerialData::Rx::GameType::UNKNOWN) + { + drivers->rtt << "game type unknown"; + if (DEBUG_HEAT_BUFFER_ENABLED) + { + drivers->rtt << " overwriting game info to game_type: " + << (int)DEBUG_HEAT_BUFFER_GAME_TYPE + << " and robot_level: " << DEBUG_HEAT_BUFFER_ROBOT_LEVEL; + game_type = DEBUG_HEAT_BUFFER_GAME_TYPE; + robot_level = DEBUG_HEAT_BUFFER_ROBOT_LEVEL; + } + drivers->rtt.endl(); + } + + if (game_type == + tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_1V1) + { + barrel_heat_buffer = BARREL_HEAT_BUFFER_1V1; + } + else if ( + game_type == + tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_3V3) + { + if (robot_level <= 0 || robot_level > 10) + { + drivers->rtt << "ERROR UNRECOGNIZED ROBOT LEVEL: " << robot_level + << ". DEFAULTING TO " << barrel_heat_buffer << " HEAT BUFFER"; + drivers->rtt.endl(); + } + else + { + barrel_heat_buffer = BARREL_HEAT_BUFFER_3V3[robot_level - 1]; + } + } + } + if (drivers->refSerial.getRefSerialReceivingData() && - power_limiter::getRemainingHeat(drivers, barrelId) <= BARREL_HEAT_BUFFER) + power_limiter::getRemainingHeat(drivers, barrelId) <= barrel_heat_buffer) { return true; } diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp index 8ffb842a..7b6f698b 100644 --- a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp +++ b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp @@ -1,5 +1,6 @@ #include "command_agitator_continuous.hpp" +using tap::communication::serial::RefSerialData; namespace commands { using tap::communication::serial::Remote; @@ -12,6 +13,59 @@ void CommandAgitatorContinuous::initialize() void CommandAgitatorContinuous::execute() { + uint16_t barrel_heat_buffer = 0.0; + if (drivers->refSerial.getRefSerialReceivingData()) + { + tap::communication::serial::RefSerialData::Rx::GameType game_type = + drivers->refSerial.getGameData().gameType; + uint8_t robot_level = drivers->refSerial.getRobotData().robotLevel; + if (game_type == RefSerialData::Rx::GameType::UNKNOWN) + { + drivers->rtt << "game type unknown"; + if (DEBUG_HEAT_BUFFER_ENABLED) + { + drivers->rtt << " overwriting game info to game_type: " + << (int)DEBUG_HEAT_BUFFER_GAME_TYPE + << " and robot_level: " << DEBUG_HEAT_BUFFER_ROBOT_LEVEL; + game_type = DEBUG_HEAT_BUFFER_GAME_TYPE; + robot_level = DEBUG_HEAT_BUFFER_ROBOT_LEVEL; + } + drivers->rtt.endl(); + } + + if (game_type == + tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_1V1) + { + barrel_heat_buffer = BARREL_HEAT_BUFFER_1V1; + } + else if ( + game_type == + tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_3V3) + { + if (robot_level <= 0 || robot_level > 10) + { + drivers->rtt << "ERROR UNRECOGNIZED ROBOT LEVEL: " << robot_level + << ". DEFAULTING TO " << barrel_heat_buffer << " HEAT BUFFER"; + drivers->rtt.endl(); + } + else + { + barrel_heat_buffer = BARREL_HEAT_BUFFER_3V3[robot_level - 1]; + } + } + } + + if (drivers->refSerial.getRefSerialReceivingData()) + { + drivers->rtt << "barrel heat: " << power_limiter::getRemainingHeat(drivers, barrelId) + << "; heat buffer: " << barrel_heat_buffer; + if (power_limiter::getRemainingHeat(drivers, barrelId) < barrel_heat_buffer) + { + drivers->rtt << " OVERHEATED"; + } + drivers->rtt.endl(); + } + // UNCOMMENT BARREL OVERHEAT ONCE WE FIX IT LMAO if (isJammed) { if (unjammingTimeout.isExpired()) @@ -24,7 +78,7 @@ void CommandAgitatorContinuous::execute() } else if ( // barrel overheat drivers->refSerial.getRefSerialReceivingData() && - power_limiter::getRemainingHeat(drivers, barrelId) <= BARREL_HEAT_BUFFER) + power_limiter::getRemainingHeat(drivers, barrelId) < barrel_heat_buffer) { jamTriggerTimeout.restart(JAM_TRIGGER_DURATION * 1000); agitator->setBallsPerSecond(0.0f); @@ -45,6 +99,7 @@ void CommandAgitatorContinuous::execute() agitator->setBallsPerSecond(BALLS_PER_SEC); } + // agitator->setBallsPerSecond(BALLS_PER_SEC); } void CommandAgitatorContinuous::end(bool) { agitator->setBallsPerSecond(0.0f); } diff --git a/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp b/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp index f3945d05..048cca0d 100644 --- a/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp @@ -41,7 +41,7 @@ void CommandMoveChassis::execute() // override spin input while beyblading if (beyblade) { - inputSpin = 1.0f; + inputSpin = 0.5f; } // rotate movement vector relative to turret @@ -61,7 +61,14 @@ bool CommandMoveChassis::applyKeyboardInput(Vector2f &inputMove, float &inputSpi { Remote *remote = &drivers->remote; - inputSpin = 0.0f; // no keyboard spin controls + if (remote->keyPressed(Remote::Key::E)) { + inputSpin = 0.5f; + } else if (remote->keyPressed(Remote::Key::Q)) { + inputSpin = -0.5f; + } else { + inputSpin = 0.0f; + } + // inputSpin = 0.0f; // no keyboard spin controls Vector2f rawMoveInput = Vector2f( remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A), diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp index da1dc92c..8605da46 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -57,23 +57,29 @@ void CommandSentryPosition::execute() // chassis->input(inputMove, inputSpin); - if (drivers->isGameActive() && moveTimer.isStopped()) - { - moveTimer.restart(5'000); // 5 secs - } + // if (drivers->isGameActive() && moveTimer.isStopped()) + // { + // moveTimer.restart(5'000); // 5 secs + // } - if (!moveTimer.isExpired()) - { - chassis->input(Vector2f(0.0f), 1.0f); // spin! - return; - } + // if (!moveTimer.isExpired()) + // { + // chassis->input(Vector2f(0.0f), 1.0f); // spin! + // return; + // } + + // PositionRequest pos = drivers->cvBoard.getPositionRequest(); + // drivers->rtt << "x_pos = " << pos.x_requested << ", y_pos = " << pos.y_requested + // << ", time_sent = " << (float)pos.time_sent; + + // // speen - PositionRequest pos = drivers->cvBoard.getPositionRequest(); - drivers->rtt << "x_pos = " << pos.x_requested << ", y_pos = " << pos.y_requested - << ", time_sent = " << (float)pos.time_sent; + // Remote *remote = &drivers->remote; + // float pwr = remote->getChannel(Remote::Channel::RIGHT_VERTICAL); + // float truePwr = pwr < ANALOG_DEAD_ZONE && !drivers->isKillSwitched() ? 0 : pwr * 0.5; + // chassis->input(Vector2f(0.0f), truePwr); - // speen - chassis->input(Vector2f(0.0f), 1.0f); + chassis->input(Vector2f(0.0f), 0.0f); } void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } diff --git a/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp new file mode 100644 index 00000000..544c651c --- /dev/null +++ b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp @@ -0,0 +1,19 @@ +#include "command_recalibrate_turret.hpp" + +namespace commands +{ + +void CommandRecalibrateTurret::initialize() {} + +void CommandRecalibrateTurret::execute() +{ + turret->setBaseYaw(turret->getMotorAngle()); + turret->setTargetWorldAngles( + turret->getCurrentLocalYaw() + turret->getChassisYaw(), + turret->getCurrentLocalPitch()); +} + +void CommandRecalibrateTurret::end(bool) {} + +bool CommandRecalibrateTurret::isFinished() const { return false; } +} // namespace commands \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp new file mode 100644 index 00000000..1e10ca7f --- /dev/null +++ b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "tap/communication/serial/remote.hpp" +#include "tap/control/command.hpp" + +#include "robots/robot_constants.hpp" +#include "subsystems/turret/turret_subsystem.hpp" + +#include "drivers.hpp" + +namespace commands +{ +using subsystems::turret::TurretSubsystem; + +class CommandRecalibrateTurret : public tap::control::Command +{ +public: + CommandRecalibrateTurret(src::Drivers* drivers, TurretSubsystem* turret) + : drivers(drivers), + turret(turret) + { + addSubsystemRequirement(turret); + } + + void initialize() override; + + void execute() override; + + void end(bool interrupted) override; + + bool isFinished() const override; + + const char* getName() const override { return "recalibrate turret command"; } + +private: + src::Drivers* drivers; + TurretSubsystem* turret; +}; +} // namespace commands \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp index 656942bc..3e754f75 100644 --- a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp +++ b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp @@ -7,14 +7,19 @@ void CommandSentryAim::initialize() {} void CommandSentryAim::execute() { // turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f); - if (turret->getTargetLocalYaw() <= -55.0f) + // if (turret->getTargetLocalYaw() <= -55.0f) + // { + // turret->setTargetWorldAngles(50.0f, 10.0f); + // } + // else + // { + // turret->setTargetWorldAngles(-50.0f, -10.0f); + // } + + if (!drivers->isKillSwitched()) { turret->setTargetWorldAngles(50.0f, 10.0f); } - else - { - turret->setTargetWorldAngles(-50.0f, -10.0f); - } /* PLAN (for suhas): diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 1e1f6d4e..232090f1 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -32,6 +32,7 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { + // drivers->rtt << "absoluate yaw enc: " << yawEncoder.getAngle() << "\n"; yaw.updateMotorAngle(); pitch.updateMotorAngle(); @@ -53,7 +54,7 @@ void TurretSubsystem::refresh() setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch()); } #else - if (!isCalibrated && !isAmputated()) + if (!isCalibrated) { baseYaw = -YAW_OFFSET; isCalibrated = true; @@ -78,6 +79,10 @@ void TurretSubsystem::refresh() } } +void TurretSubsystem::setBaseYaw(float by) { baseYaw = by; } + +float TurretSubsystem::getMotorAngle() { return -yaw.getAngle(); } + void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration) { targetPosition = position; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index 90a92433..65846b9e 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp @@ -37,6 +37,9 @@ class TurretSubsystem : public Subsystem float getCurrentLocalYaw(); float getCurrentLocalPitch(); bool getIsCalibrated(); + float getMotorAngle(); + + void setBaseYaw(float by); private: src::Drivers* drivers;