diff --git a/taproot b/taproot index 22b708e6..cd047d2f 160000 --- a/taproot +++ b/taproot @@ -1 +1 @@ -Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f +Subproject commit cd047d2fd6d932320d03390e6e6e3fff4fcb38fd diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp index db545381..584b1b51 100644 --- a/ut-robomaster/src/communication/cv_board.cpp +++ b/ut-robomaster/src/communication/cv_board.cpp @@ -24,6 +24,11 @@ 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_POSITION_REQUEST: + decodePositionRequest(message); + break; default: break; } @@ -35,6 +40,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; @@ -77,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 89e00d0e..9949cade 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 */ @@ -61,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 @@ -84,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 263164df..146f406c 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,8 @@ 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, + CV_MESSAGE_TYPE_POSITION_REQUEST = 5, }; enum ColorTypes : uint8_t @@ -35,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/main.cpp b/ut-robomaster/src/main.cpp index 21f43fe3..b48046cf 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" @@ -24,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 @@ -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(); diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp index 0af76a76..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 ------------------------------------------------ @@ -41,10 +43,12 @@ 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 = 1.0f; +static constexpr float YAW_REDUCTION = 2.005459491f; static constexpr float PITCH_REDUCTION = 1.0f; // Tuning Constants ----------------------------------- @@ -95,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 ------------------------------------- @@ -105,28 +109,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, {}}; -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, {}}; -const float YAW_OFFSET = 0; -const float PITCH_OFFSET = 0; + PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false}; // Velocities ---------------------------- @@ -153,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 b4c0d2d2..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 ------------------------------------------------ @@ -25,8 +27,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; @@ -100,14 +102,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 +128,8 @@ const MotorConfig FLYWHEEL_TR{ false, "flywheel top right", PID_FLYWHEEL, - {}}; + {}, + false}; const MotorConfig FLYWHEEL_BL{ M3508_NOGEARBOX, MOTOR5, @@ -123,7 +137,8 @@ const MotorConfig FLYWHEEL_BL{ false, "flywheel bottom left", PID_FLYWHEEL, - {}}; + {}, + false}; const MotorConfig FLYWHEEL_BR{ M3508_NOGEARBOX, MOTOR6, @@ -131,15 +146,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; @@ -167,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/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/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp index 86f44722..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 = 0.0f; +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 -------------------------------- @@ -105,27 +108,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 - FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL, {}}; + 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_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 ------------------------------------- @@ -144,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/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/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/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp index e4bfa65e..0d71b82d 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() @@ -106,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) @@ -118,6 +123,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..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 { @@ -36,11 +37,14 @@ 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; 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_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 7feef19e..8605da46 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp @@ -3,27 +3,150 @@ namespace commands { +using namespace communication; +// old ard code void CommandSentryPosition::initialize() { moveTimer.stop(); } +// void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); } + void CommandSentryPosition::execute() { - // wait until game starts then wait some more to avoid excess drifting - if (drivers->isGameActive() && moveTimer.isStopped()) + // float yawAngle = turret->getTargetLocalYaw(); + // Vector2f inputMove = Vector2f(0.0f); + // float inputSpin = 0.0f; + + // // get keyboard input + // float keyboardInputSpin; + // bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin); + + // // 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; + // } + + // // 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; + // } + + // // rotate movement vector relative to turret + // if (turretRelative) + // { + // inputMove = inputMove.rotate(yawAngle); + // } + + // chassis->input(inputMove, inputSpin); + + // if (drivers->isGameActive() && moveTimer.isStopped()) + // { + // moveTimer.restart(5'000); // 5 secs + // } + + // 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 + + // 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); + + chassis->input(Vector2f(0.0f), 0.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; + + 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) { - moveTimer.restart(10'000); // 10s + Vector2f moveDir = rawMoveInput / rawInputLen; // normalize input + inputMove += moveDir * KEYBOARD_ACCEL * DT; // incorporate input + inputMove /= max(1.0f, inputMove.getLength()); // clamp length } - - if (!moveTimer.isExpired()) + else { - chassis->input(Vector2f(0.0f), 0.0f); - return; + // decelerate when input stops + float len = inputMove.getLength(); + if (len > 0.0f) + { + inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f); + } } - // speen - chassis->input(Vector2f(0.0f), 1.0f); + return rawMoveInput != Vector2f(0.0f); } -void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } -bool CommandSentryPosition::isFinished() const { return false; } +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..546fc89c 100644 --- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp +++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp @@ -1,9 +1,15 @@ #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 +17,23 @@ namespace commands using namespace tap::communication::serial; using namespace modm; using subsystems::chassis::ChassisSubsystem; +using subsystems::turret::TurretSubsystem; using tap::arch::MilliTimeout; 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); } @@ -37,5 +52,13 @@ class CommandSentryPosition : public tap::control::Command src::Drivers *drivers; ChassisSubsystem *chassis; 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 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/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/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 895c9b91..3e754f75 100644 --- a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp +++ b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp @@ -7,6 +7,32 @@ 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); + // } + + if (!drivers->isKillSwitched()) + { + 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) {} diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp index 9b06d677..11f3609d 100644 --- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp @@ -34,27 +34,28 @@ void DoubleYawMotor::reset() void DoubleYawMotor::updateMotorAngle() { - float encoderAngle = static_cast(motor1.getEncoderUnwrapped()) / - DjiMotor::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; - currentAngle.setValue(encoderAngle); + float encoderAngle = + static_cast(motor1.getInternalEncoder().getEncoder().getUnwrappedValue()) / + DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION; + 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 - 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); } -float DoubleYawMotor::getAngle() { return currentAngle.getValue() * M_TWOPI; } +float DoubleYawMotor::getAngle() { return currentAngle.getWrappedValue() * M_TWOPI; } void DoubleYawMotor::setVelocity(float velocity, float dt) { @@ -64,8 +65,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; } @@ -78,6 +79,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 8273a68a..e6c075e1 100644 --- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp @@ -9,8 +9,14 @@ 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, + motorConfig.currentControl), + drivers(drivers), pid(pidConfig), setpoint(0.0f, 0.0f, M_TWOPI), currentAngle(0.0f, 0.0f, M_TWOPI) @@ -27,32 +33,35 @@ void TurretMotor::reset() void TurretMotor::updateMotorAngle() { - uint16_t encoderValue = motor.getEncoderWrapped(); + // uint16_t encoderValue = motor.getEncoderWrapped(); + uint16_t encoderValue = motor.getInternalEncoder().getEncoder().getUnwrappedValue(); if (lastUpdatedEncoderValue != encoderValue) { lastUpdatedEncoderValue = encoderValue; unwrappedAngle = static_cast(encoderValue) * M_TWOPI / - static_cast(DjiMotor::ENC_RESOLUTION); - currentAngle.setValue(unwrappedAngle); + static_cast(DjiMotorEncoder::ENC_RESOLUTION); + 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 output = - pid.runController(positionControllerError, (M_TWOPI / 60.0f) * motor.getShaftRPM(), dt); + float positionControllerError = WrappedFloat(currentAngle.getWrappedValue(), 0, M_TWOPI) + .minDifference(setpoint.getWrappedValue()); + float output = pid.runController( + positionControllerError, + (M_TWOPI / 60.0f) * motor.getInternalEncoder().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 12fcc8d2..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" @@ -27,13 +27,15 @@ class TurretMotor float getSetpoint(); bool isOnline(); + DjiMotor motor; + private: src::Drivers *drivers; - DjiMotor motor; + // 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.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 37f22cf3..232090f1 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,23 +32,29 @@ void TurretSubsystem::initialize() void TurretSubsystem::refresh() { - setAmputated(!hardwareOk()); - + // drivers->rtt << "absoluate yaw enc: " << yawEncoder.getAngle() << "\n"; yaw.updateMotorAngle(); pitch.updateMotorAngle(); + drivers->rtt << "pitch: " << pitch.getAngle(); + drivers->rtt.endl(); + #if defined(TARGET_STANDARD) || defined(TARGET_HERO) yawEncoder.update(); - if (!isCalibrated && !isAmputated() && yawEncoder.isOnline()) + 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()); } #else - if (!isCalibrated && !isAmputated()) + if (!isCalibrated) { baseYaw = -YAW_OFFSET; isCalibrated = true; @@ -54,10 +63,14 @@ void TurretSubsystem::refresh() } #endif - if (isCalibrated && !drivers->isKillSwitched() && !isAmputated()) + 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 { @@ -66,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; @@ -79,7 +96,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/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index 630e02eb..65846b9e 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 { @@ -37,6 +37,9 @@ class TurretSubsystem : public Subsystem float getCurrentLocalYaw(); float getCurrentLocalPitch(); bool getIsCalibrated(); + float getMotorAngle(); + + void setBaseYaw(float by); private: src::Drivers* drivers; 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.cpp b/ut-robomaster/src/utils/motors/motor_controller.cpp index f0cc69ba..74723ebd 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.getInternalEncoder().getEncoder().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; } 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) { 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..6051dd71 --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp @@ -0,0 +1,484 @@ +/*****************************************************************************/ +/********** !!! 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" + +/** + * @class Butterworth + * @brief Implementation of Butterworth filter design in the discrete domain. + * + * 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. + * + * 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``, enabling + * compile-time computation of filter configurations. + * + * @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 + * static constexpr Butterworth<1, LOWPASS> filter(wc, Ts); + * auto naturalCoeffs = filter.getNaturalResponseCoefficients(); + * auto forcedCoeffs = filter.getForcedResponseCoefficients(); + * DiscreteFilter<2> Filter(naturalCoeffs, forcedCoeffs); + * + * @endcode + * + * @author Aiden Prevey + * @date 4/29/2025 + * @version 2.0 + */ + +namespace tap +{ +namespace algorithms +{ +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 (1.0 + (Ts / 2) * s) / (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 complex_abs(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 complex_sqrt(std::complex z) +{ + 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)}; +} + +template +class Butterworth +{ +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; + + // 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)); + } + + std::array, COEFFICIENTS - 1> zPoles; + + // apply the appropriate s-domain 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: + { + // 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; + } + + 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"); + } + + 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 = complex_abs(freqResp); + double scale = 1 / mag; + for (auto &coef : b) coef *= scale; + break; + } + 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: + { + // 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; + } + } + + // 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]; + } + } + + static constexpr int COEFFICIENTS = (1 + ((Type & 0b10) != 0)) * ORDER + 1; + + std::array getNaturalResponseCoefficients() const + { + return naturalResponseCoefficients; + } + + std::array getForcedResponseCoefficients() const + { + return forcedResponseCoefficients; + } + +private: + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; +}; + +} // namespace algorithms + +} // namespace tap + +#endif // TAPROOT_BUTTERWORTH_HPP_ \ No newline at end of file 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/discrete_filter.hpp b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp new file mode 100644 index 00000000..363ad69b --- /dev/null +++ b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp @@ -0,0 +1,135 @@ +/*****************************************************************************/ +/********** !!! 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 +{ +namespace algorithms +{ +/** + * @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(); + } + + /** + * @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; + } + +private: + std::array naturalResponseCoefficients; + std::array forcedResponseCoefficients; + std::array naturalResponse; + std::array forcedResponse; +}; + +} // namespace algorithms + +} // namespace tap + +#endif // TAPROOT_DISCRETE_FILTER_HPP_ \ No newline at end of file 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/algorithms/odometry/odometry_2d_interface.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp index 71332bdd..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 @@ -62,6 +62,8 @@ class Odometry2DInterface * @return The last time that odometry was computed (in microseconds). */ virtual uint32_t getLastComputedOdometryTime() const = 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.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..317ff7dc --- /dev/null +++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp @@ -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 . + */ + +#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); + } + + pastPosition = position; + deltaTime = tap::arch::clock::getTimeMicroseconds() - lastUpdateTime; + lastUpdateTime = tap::arch::clock::getTimeMicroseconds(); + 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..83b091ea --- /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. + */ + uint64_t lastUpdateTime; + + uint64_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..ba33c9fe --- /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; +} + +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..84bb24c4 --- /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) + { + } + + 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(); + + /** + * 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 applyTransform(ImuData& data) + { + data.accG = mountingTransform.apply(data.accG); + data.gyroRadPerSec = mountingTransform.apply(data.gyroRadPerSec); + } + + 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..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 @@ -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,45 @@ 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); 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; + + applyTransform(imuData); + + 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..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 @@ -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 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. 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..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 @@ -56,57 +56,52 @@ 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; - - /** - * Returns roll angle in degrees. - */ - virtual inline float getRoll() = 0; + virtual inline float getRoll() const = 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/algorithms/linear_interpolation_predictor_contiguous.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp similarity index 54% rename from ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp rename to ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp index a0b20378..59a7b86e 100644 --- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp +++ b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp @@ -21,35 +21,24 @@ * along with Taproot. If not, see . */ -#include "linear_interpolation_predictor_contiguous.hpp" +#ifndef TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_ +#define TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_ -namespace tap::algorithms -{ -LinearInterpolationPredictorContiguous::LinearInterpolationPredictorContiguous( - float lowerBound, - float upperBound) - : lastUpdateCallTime(0), - previousValue(0.0f, lowerBound, upperBound), - slope(0.0f) -{ -} +#include "tap/communication/sensors/sensor_interface.hpp" -void LinearInterpolationPredictorContiguous::update(float newValue, uint32_t currTime) +namespace tap::communication::sensors::voltage { - 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) +/** + * Interface for a generic voltage sensor. + */ +class VoltageSensorInterface : public tap::communication::sensors::SensorInterface { - previousValue.setValue(initialValue); - lastUpdateCallTime = initialTime; - slope = 0.0f; -} -} // namespace tap::algorithms +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..11c330f9 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 }; /** 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..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 @@ -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. }; }; 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..4995dcb4 --- /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 + 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; + + /** + * @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 9de73908..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; @@ -113,8 +131,11 @@ 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 (can1ValidMotorMessage6020Current) + { + messageSuccess &= + drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1Message6020Current); } } if (drivers->can.isReadyToSend(can::CanBus::CAN_BUS2)) @@ -127,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) @@ -139,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++) { @@ -153,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..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 @@ -37,6 +37,11 @@ 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)); }; } // namespace tap::mock