From 067bc7dc4153e5375d3f655ff01113e7e364e1a8 Mon Sep 17 00:00:00 2001 From: IsabellaSantacruz Date: Mon, 18 Aug 2025 15:02:58 -0400 Subject: [PATCH 01/12] add robot config to chassis subsystem --- .../src/control/robot_config.hpp | 32 +++ .../chassis/chassis_spin2win_subsystem.cpp | 201 ------------------ .../chassis/chassis_spin2win_subsystem.hpp | 149 ------------- .../subsystems/chassis/chassis_subsystem.cpp | 33 ++- .../subsystems/chassis/chassis_subsystem.hpp | 15 +- 5 files changed, 63 insertions(+), 367 deletions(-) create mode 100644 PolySTAR-Taproot-project/src/control/robot_config.hpp delete mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp delete mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp diff --git a/PolySTAR-Taproot-project/src/control/robot_config.hpp b/PolySTAR-Taproot-project/src/control/robot_config.hpp new file mode 100644 index 0000000..2bf6b9a --- /dev/null +++ b/PolySTAR-Taproot-project/src/control/robot_config.hpp @@ -0,0 +1,32 @@ +enum DriveType +{ + relative, + nonRelative +}; + +enum WheelType +{ + omniwheel, + mecanum +}; + +enum MovementType +{ + spin2win, + normal +}; + +enum ControlType +{ + keyboard, + controller, + keyboardAndController +}; + +struct SRobotConfig +{ + DriveType driveType; + WheelType wheelType; + MovementType movementType; + ControlType controlType; +}; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp deleted file mode 100644 index 0bf9ec0..0000000 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp +++ /dev/null @@ -1,201 +0,0 @@ -#include "chassis_spin2win_subsystem.hpp" - -#include "tap/communication/serial/remote.hpp" -#include "tap/algorithms/math_user_utils.hpp" -#include "control/drivers/drivers.hpp" -#include "communication/cv_handler.hpp" - -using namespace tap; -using tap::communication::serial::Uart; - -namespace control -{ -namespace chassis -{ - -void ChassisSpin2WinSubsystem::initialize() -{ - frontLeftMotor.initialize(); - frontRightMotor.initialize(); - backLeftMotor.initialize(); - backRightMotor.initialize(); - prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); -} - -void ChassisSpin2WinSubsystem::refresh() { - updateRpmSetpoints(); - - uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevPidUpdate; - updateRpmPid(&frontLeftPid, &frontLeftMotor, frontLeftDesiredRpm, dt); - updateRpmPid(&frontRightPid, &frontRightMotor, frontRightDesiredRpm, dt); - updateRpmPid(&backLeftPid, &backLeftMotor, backLeftDesiredRpm, dt); - updateRpmPid(&backRightPid, &backRightMotor, backRightDesiredRpm, dt); - prevPidUpdate = tap::arch::clock::getTimeMilliseconds(); - - // Attempt to send a UART positionMessage to Jetson if the delay has elapsed - if (tap::arch::clock::getTimeMilliseconds() - prevCVUpdate > CHASSIS_CV_UPDATE_PERIOD ) { - prevCVUpdate = tap::arch::clock::getTimeMilliseconds(); - sendCVUpdate(); - } - - if (CHASSIS_DEBUG_MESSAGE == false) return; - - if (tap::arch::clock::getTimeMilliseconds() - prevDebugTime > CHASSIS_DEBUG_MESSAGE_DELAY_MS) { - prevDebugTime = tap::arch::clock::getTimeMilliseconds(); - char buffer[500]; - - // Front right debug message - int nBytes = sprintf (buffer, "FR-RPM: %i, SETPOINT: %i\n", - frontRightMotor.getShaftRPM(), - (int)frontRightDesiredRpm); - drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - // Front left debug message - nBytes = sprintf (buffer, "FL-RPM: %i, SETPOINT: %i\n", - frontLeftMotor.getShaftRPM(), - (int)frontLeftDesiredRpm); - drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - // Back right debug message - nBytes = sprintf (buffer, "BR-RPM: %i, SETPOINT: %i\n", - backRightMotor.getShaftRPM(), - (int)backRightDesiredRpm); - drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - // Back left debug message - nBytes = sprintf (buffer, "BL-RPM: %i, SETPOINT: %i\n", - backLeftMotor.getShaftRPM(), - (int)backLeftDesiredRpm); - drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - } -} - -void ChassisSpin2WinSubsystem::updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt) { - int64_t error = desiredRpm - motor->getShaftRPM(); - pid->runControllerDerivateError(error, dt); - if (desiredRpm == 0) { - motor->setDesiredOutput(0); - } else { - motor->setDesiredOutput(pid->getOutput()); - } -} - -void ChassisSpin2WinSubsystem::updateRpmSetpoints() { - uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevRampUpdate; - - if(xInputRamp.isTargetReached() == false) { xInputRamp.update(RAMP_SLOPE * dt); } - if(yInputRamp.isTargetReached() == false) { yInputRamp.update(RAMP_SLOPE * dt); } - if(rInputRamp.isTargetReached() == false) { rInputRamp.update(RAMP_SLOPE * dt); } - - setDesiredOutput(xInputRamp.getValue(), yInputRamp.getValue(), rInputRamp.getValue()); - prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); -} - -void ChassisSpin2WinSubsystem::setTargetOutput(float x, float y, float r) { - xInputRamp.setTarget(x); - yInputRamp.setTarget(y); - rInputRamp.setTarget(r); -} - -/* - Give desired setpoints for chassis movement. - +x is forward, +y is right, +r is clockwise (turning right). - Expressed in body frame. -*/ -void ChassisSpin2WinSubsystem::setDesiredOutput(float x, float y, float r) -{ - - x = tap::algorithms::limitVal(x,-1,1); - y = tap::algorithms::limitVal(y,-1,1); - r = tap::algorithms::limitVal(r,-1,1); - - // x, y, and r contained between -1 and 1 - // Normalize movement vector - float norm = sqrt(x*x+y*y); - if (norm > 1) { - x = x / norm; - y = y / norm; - } - - y = IS_Y_INVERTED ? -y : y; - - float frontLeftValue = y + r; - float frontRightValue = -x - r; - float backLeftValue = -x + r; - float backRightValue = y - r; - - frontLeftDesiredRpm = frontLeftValue * rpmScaleFactor; - frontRightDesiredRpm = frontRightValue * rpmScaleFactor; - backLeftDesiredRpm = backLeftValue * rpmScaleFactor; - backRightDesiredRpm = backRightValue * rpmScaleFactor; -} - -/* - Attempts to send IMU and wheel encoder data to CV over UART. - Returns true if the positionMessage was sent sucessfully. -*/ -void ChassisSpin2WinSubsystem::sendCVUpdate() { - - // Get IMU measurements - float Ax = drivers->mpu6500.getAx(); - float Ay = drivers->mpu6500.getAy(); - float Az = drivers->mpu6500.getAz(); - float Gx = drivers->mpu6500.getGx(); - float Gy = drivers->mpu6500.getGy(); - float Gz = drivers->mpu6500.getGz(); - float Rx = drivers->mpu6500.getRoll(); - float Ry = drivers->mpu6500.getPitch(); - float Rz = drivers->mpu6500.getYaw(); - - // Get motor encoder positions - // Revolutions is calculated because DJIMotor interface does not have the getter for this value - uint16_t frontLeftEncoder = frontLeftMotor.getEncoderWrapped(); - int16_t frontLeftRevolutions = (frontLeftMotor.getEncoderUnwrapped() - frontLeftEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; - uint16_t frontRightEncoder = frontRightMotor.getEncoderWrapped(); - int16_t frontRightRevolutions = (frontRightMotor.getEncoderUnwrapped() - frontRightEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; - uint16_t backLeftEncoder = backLeftMotor.getEncoderWrapped(); - int16_t backLeftRevolutions = (backLeftMotor.getEncoderUnwrapped() - backLeftEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; - uint16_t backRightEncoder = backRightMotor.getEncoderWrapped(); - int16_t backRightRevolutions = (backRightMotor.getEncoderUnwrapped() - backRightEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; - - // Get motor RPMs - int16_t frontLeftRPM = frontLeftMotor.getShaftRPM(); - int16_t frontRightRPM = frontRightMotor.getShaftRPM(); - int16_t backLeftRPM = backLeftMotor.getShaftRPM(); - int16_t backRightRPM = backRightMotor.getShaftRPM(); - - // Convert IMU and encoder data to 2 byte data types for transmission - // Conversions need to occur to respect 2 byte limit for each value sent - // Accelerations : converted from m/s2 to int16_t mm/s2 - // Gyro : converted from deg/s to int16_t millirad/s - // Attitude : converted from deg to int16_t millirad - // Encoder positions: passed as is - // Encoder revolutions: converted to int16_t - // Encoder RPM: passed as is - src::communication::cv::CVSerialData::Tx::PositionMessage positionMessage; - positionMessage.Ax = static_cast(Ax*M_TO_MM); - positionMessage.Ay = static_cast(Ay*M_TO_MM); - positionMessage.Az = static_cast(Az*M_TO_MM); - positionMessage.Gx = static_cast(Gx*DEG_TO_MILLIRAD); - positionMessage.Gy = static_cast(Gy*DEG_TO_MILLIRAD); - positionMessage.Gz = static_cast(Gz*DEG_TO_MILLIRAD); - positionMessage.Rx = static_cast(Rx*DEG_TO_MILLIRAD); - positionMessage.Ry = static_cast(Ry*DEG_TO_MILLIRAD); - positionMessage.Rz = static_cast(Rz*DEG_TO_MILLIRAD); - positionMessage.frontLeftEncoder = frontLeftEncoder; - positionMessage.frontLeftRevolutions = frontLeftRevolutions; - positionMessage.frontRightEncoder = frontRightEncoder; - positionMessage.frontRightRevolutions = frontRightRevolutions; - positionMessage.backLeftEncoder = backLeftEncoder; - positionMessage.backLeftRevolutions = backLeftRevolutions; - positionMessage.backRightEncoder = backRightEncoder; - positionMessage.backRightRevolutions = backRightRevolutions; - positionMessage.frontLeftRPM = frontLeftRPM; - positionMessage.frontRightRPM = frontRightRPM; - positionMessage.backLeftRPM = backLeftRPM; - positionMessage.backRightRPM = backRightRPM; - - drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&positionMessage), sizeof(positionMessage)); -} - -} // namespace chassis - -} // namespace control - diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp deleted file mode 100644 index e4adb5e..0000000 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp +++ /dev/null @@ -1,149 +0,0 @@ -#ifndef CHASSIS_SPIN2WIN_SUBSYSTEM_HPP_ -#define CHASSIS_SPIN2WIN_SUBSYSTEM_HPP_ - -#include "tap/control/subsystem.hpp" -#include "tap/algorithms/smooth_pid.hpp" -#include "modm/math/filter/pid.hpp" -#include "tap/algorithms/ramp.hpp" -#include "tap/motor/dji_motor.hpp" -#include "tap/util_macros.hpp" -#include "chassis_constants.hpp" -#include "control/drivers/drivers.hpp" - -//#include "control/control_operator_interface_edu.hpp" - -namespace control -{ -namespace chassis -{ -/** - * A bare bones Subsystem for interacting with a 4 wheeled chassis. - */ -class ChassisSpin2WinSubsystem : public tap::control::Subsystem -{ -public: - /** - * This max output is measured in the c620 robomaster translated current. - * Per the datasheet, the controllable current range is -16384 ~ 0 ~ 16384. - * The corresponding speed controller output torque current range is - * -20 ~ 0 ~ 20 A. - * - * For this demo, we have capped the output at 8000. This should be more - * than enough for what you are doing. - */ - static constexpr float MAX_CURRENT_OUTPUT = 8000.0f; - - /** - * Constructs a new ChassisSpin2WinSubsystem with default parameters specified in - * the private section of this class. - */ - ChassisSpin2WinSubsystem(src::Drivers *drivers) - : tap::control::Subsystem(drivers), - drivers(drivers), - frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), - frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), - backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), - backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "back right motor"), - frontLeftPid(pidConfig), - frontRightPid(pidConfig), - backLeftPid(pidConfig), - backRightPid(pidConfig), - frontLeftDesiredRpm(0), - frontRightDesiredRpm(0), - backLeftDesiredRpm(0), - backRightDesiredRpm(0), - prevCVUpdate(0) - { - } - - ChassisSpin2WinSubsystem(const ChassisSpin2WinSubsystem &other) = delete; - - ChassisSpin2WinSubsystem &operator=(const ChassisSpin2WinSubsystem &other) = delete; - - ~ChassisSpin2WinSubsystem() = default; - - void initialize() override; - - void refresh() override; - - void setDesiredOutput(float x, float y, float r); - - void updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt); - void updateRpmSetpoints(); - void setTargetOutput(float x, float y, float r); - - void sendCVUpdate(); - - const tap::motor::DjiMotor &getFrontLeftMotor() const { return frontLeftMotor; } - const tap::motor::DjiMotor &getFrontRightMotor() const { return frontRightMotor; } - const tap::motor::DjiMotor &getBackLeftMotor() const { return backLeftMotor; } - const tap::motor::DjiMotor &getBackRightMotor() const { return backRightMotor; } - -private: - src::Drivers *drivers; - - ///< Hardware constants, not specific to any particular chassis. - static constexpr tap::motor::MotorId FRONT_LEFT_MOTOR_ID = tap::motor::MOTOR1; - static constexpr tap::motor::MotorId FRONT_RIGHT_MOTOR_ID = tap::motor::MOTOR2; - static constexpr tap::motor::MotorId BACK_RIGHT_MOTOR_ID = tap::motor::MOTOR3; - static constexpr tap::motor::MotorId BACK_LEFT_MOTOR_ID = tap::motor::MOTOR4; - static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; - - ///< Motors. Use these to interact with any dji style motors. - tap::motor::DjiMotor frontLeftMotor; - tap::motor::DjiMotor frontRightMotor; - tap::motor::DjiMotor backLeftMotor; - tap::motor::DjiMotor backRightMotor; - - // Smooth PID configuration - tap::algorithms::SmoothPidConfig pidConfig = { CHASSIS_PID_KP, CHASSIS_PID_KI, CHASSIS_PID_KD, - CHASSIS_PID_MAX_ERROR_SUM, CHASSIS_PID_MAX_OUTPUT, - CHASSIS_TQ_DERIVATIVE_KALMAN, CHASSIS_TR_DERIVATIVE_KALMAN, - CHASSIS_TQ_PROPORTIONAL_KALMAN, CHASSIS_TR_PROPORTIONAL_KALMAN }; - - // Smooth PID controllers for position feedback from motors - tap::algorithms::SmoothPid frontLeftPid; - tap::algorithms::SmoothPid frontRightPid; - tap::algorithms::SmoothPid backLeftPid; - tap::algorithms::SmoothPid backRightPid; - - ///< Any user input is translated into desired RPM for each motor. - float frontLeftDesiredRpm; - float frontRightDesiredRpm; - float backLeftDesiredRpm; - float backRightDesiredRpm; - - // Ramp for each input - tap::algorithms::Ramp xInputRamp; - tap::algorithms::Ramp yInputRamp; - tap::algorithms::Ramp rInputRamp; - - // previous update time for ramp - float prevRampUpdate = 0.0f; - - // Ramp time - static constexpr float RAMP_TIME_MS = 500.0f; - - // Slope for ramp - static constexpr float RAMP_SLOPE = 1.0f / RAMP_TIME_MS; - - // Scale factor for converting joystick movement into RPM setpoint - static constexpr float rpmScaleFactor = 3500.0f; - - uint32_t prevDebugTime; - uint32_t prevPidUpdate; - - // Variables for managing UART messages sent to CV - uint32_t prevCVUpdate; - - // Conversions for CV Messages - const int16_t M_TO_MM = 1000; - const float DEG_TO_MILLIRAD = 17.453293; - -}; // class ChassisSpin2WinSubsystem - -} // namespace chassis - -} // namespace control - -#endif // CHASSIS_SUBSYSTEM_HPP_ diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index d3c7a63..3cb07f1 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -64,12 +64,6 @@ void ChassisSubsystem::refresh() { backLeftMotor.getShaftRPM(), (int)backLeftDesiredRpm); drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - //rotation angle debug message - nBytes = sprintf (buffer, "RO-AGL: %f, SETPOINT: %i\n", - (double)rotationAngle, - (int)0); - drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); - } } @@ -122,10 +116,15 @@ void ChassisSubsystem::setDesiredOutput(float x, float y, float r) y = IS_Y_INVERTED ? -y : y; - frontLeftDesiredRpm = (x-y-r)*rpmScaleFactor; - frontRightDesiredRpm = (x+y+r)*rpmScaleFactor; - backLeftDesiredRpm = (x+y-r)*rpmScaleFactor; - backRightDesiredRpm = (x-y+r)*rpmScaleFactor; + switch (wheelType) { + case WheelType::omniwheel: + setOmniwheelDesiredRPM(x, y, r); + break; + case WheelType::mecanum: + default: + setMecanumDesiredRPM(x, y, r); + break; + } } /* @@ -196,6 +195,20 @@ void ChassisSubsystem::sendCVUpdate() { drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&positionMessage), sizeof(positionMessage)); } +void ChassisSubsystem::setMecanumDesiredRPM(const float& x, const float& y, const float& r) { + frontLeftDesiredRpm = (x-y-r)*rpmScaleFactor; + frontRightDesiredRpm = (x+y+r)*rpmScaleFactor; + backLeftDesiredRpm = (x+y-r)*rpmScaleFactor; + backRightDesiredRpm = (x-y+r)*rpmScaleFactor; +} + +void ChassisSubsystem::setOmniwheelDesiredRPM(const float& x, const float& y, const float& r) { + frontLeftDesiredRpm = (y+r) * rpmScaleFactor; + frontRightDesiredRpm = (-x-r) * rpmScaleFactor; + backLeftDesiredRpm = (-x+r) * rpmScaleFactor; + backRightDesiredRpm = (y-r) * rpmScaleFactor; +} + } // namespace chassis } // namespace control diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index aa11674..58a7154 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -9,6 +9,7 @@ #include "tap/util_macros.hpp" #include "chassis_constants.hpp" #include "control/drivers/drivers.hpp" +#include "control/robot_config.hpp" //#include "control/control_operator_interface_edu.hpp" @@ -37,9 +38,10 @@ class ChassisSubsystem : public tap::control::Subsystem * Constructs a new ChassisSubsystem with default parameters specified in * the private section of this class. */ - ChassisSubsystem(src::Drivers *drivers) + ChassisSubsystem(src::Drivers *drivers, const SRobotConfig& robotConfig) : tap::control::Subsystem(drivers), drivers(drivers), + wheelType(robotConfig.wheelType), frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), @@ -79,12 +81,14 @@ class ChassisSubsystem : public tap::control::Subsystem const tap::motor::DjiMotor &getBackLeftMotor() const { return backLeftMotor; } const tap::motor::DjiMotor &getBackRightMotor() const { return backRightMotor; } - float getRotationAngle(){ return rotationAngle;} - void setRotationAngle(float newRotationAngle){ rotationAngle = newRotationAngle;} - private: + void setMecanumDesiredRPM(const float& x, const float& y, const float& r); + void setOmniwheelDesiredRPM(const float& x, const float& y, const float& r); + src::Drivers *drivers; + WheelType wheelType; + ///< Hardware constants, not specific to any particular chassis. static constexpr tap::motor::MotorId FRONT_LEFT_MOTOR_ID = tap::motor::MOTOR1; static constexpr tap::motor::MotorId FRONT_RIGHT_MOTOR_ID = tap::motor::MOTOR2; @@ -142,9 +146,6 @@ class ChassisSubsystem : public tap::control::Subsystem const int16_t M_TO_MM = 1000; const float DEG_TO_MILLIRAD = 17.453293; - //variable used for spin2win debugging - float rotationAngle = 0.0f; - }; // class ChassisSubsystem } // namespace chassis From e6f54b12ac4208df73f794a651cd18b9552d67f8 Mon Sep 17 00:00:00 2001 From: antoinefischer Date: Sat, 11 Oct 2025 13:58:04 -0400 Subject: [PATCH 02/12] LQR logic implemented (untested) --- .../chassis/algorithms/chassis_lqr.cpp | 56 +++++++ .../chassis/algorithms/chassis_lqr.hpp | 57 +++++++ .../subsystems/chassis/chassis_subsystem.cpp | 149 +++++++++++------- .../subsystems/chassis/chassis_subsystem.hpp | 82 ++++++---- .../turret/algorithms/turret_lqr.cpp | 40 +++++ .../turret/algorithms/turret_lqr.hpp | 39 +++++ .../subsystems/turret/turret_constants.hpp | 13 +- .../subsystems/turret/turret_subsystem.cpp | 103 ++++++------ .../subsystems/turret/turret_subsystem.hpp | 26 +-- 9 files changed, 422 insertions(+), 143 deletions(-) create mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp create mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp create mode 100644 PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp create mode 100644 PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp new file mode 100644 index 0000000..8ebb6f7 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -0,0 +1,56 @@ +#include "chassis_lqr.hpp" +#include // std::clamp + +namespace control +{ +namespace chassis::algorithms +{ + +ChassisLqrController::ChassisLqrController(float mass, + float inertia, + float motorOutputMax, + float axisToMotorScale) + : m_(mass), + I_(inertia), + maxOut_(motorOutputMax), + scale_(axisToMotorScale) +{ + // Default conservative gains. Replace these values via setGains() whenever LQR K values need to be recomputed if big changes have been made to robot. + Kx_ = defaultGains(m_); + Ky_ = defaultGains(m_); + Kt_ = defaultGains(I_); +} + +void ChassisLqrController::setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt) +{ + Kx_ = Kx; + Ky_ = Ky; + Kt_ = Kt; +} + +std::array ChassisLqrController::update(float vx, float dvx, float vx_ref, + float vy, float dvy, float vy_ref, + float w, float dw, float w_ref) +{ + const float ux = -(Kx_.k_pos * (vx - vx_ref) + Kx_.k_vel * dvx); + const float uy = -(Ky_.k_pos * (vy - vy_ref) + Ky_.k_vel * dvy); + const float ut = -(Kt_.k_pos * (w - w_ref ) + Kt_.k_vel * dw ); + + // Omni mapping replacing setOmniwheelDesiredRPM: + // FrontLeft = +y + r, FrontRight = -x - r, BackLeft = -x + r, BackRight = +y - r + float fl = ( uy + ut) * scale_; + float fr = (-ux - ut) * scale_; + float bl = (-ux + ut) * scale_; + float br = ( uy - ut) * scale_; + + auto clamp = [this](float v) { return std::clamp(v, -maxOut_, +maxOut_); }; + return { clamp(fl), clamp(fr), clamp(bl), clamp(br) }; +} + +ChassisLqrController::Gains2 ChassisLqrController::defaultGains(float dyn) +{ + return { 1.5f * dyn, 0.1f }; +} + +} // namespace chassis::algorithms +} // namespace control diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp new file mode 100644 index 0000000..97ed8aa --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -0,0 +1,57 @@ +#ifndef CHASSIS_LQR_HPP_ +#define CHASSIS_LQR_HPP_ + +#include + +namespace control +{ +namespace chassis::algorithms +{ + +/* LQR controller for 4-wheel omni chassis. + Regulates body-frame velocities (vx, vy, w) to ramp/joystick references. + Produces 4 motor commands (FL, FR, BL, BR) in motor units. */ +class ChassisLqrController +{ +public: + struct Gains2 { float k_pos; float k_vel; }; + + /* mass: chassis mass (kg) + inertia: chassis yaw inertia about CoM (kg·m²) + motorOutputMax: clamp for motor command (C620 DJI currently) + axisToMotorScale: maps axis effort to per-wheel command (rpmScaleFactor) */ + ChassisLqrController(float mass, + float inertia, + float motorOutputMax = 8000.0f, + float axisToMotorScale = 3500.0f); + + // Optional: inject gains computed offline (CARE). + void setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt); + + /* Update controller. + Inputs are CURRENT body velocities and references, normalized to [-1,1] (state-based control). + Derivatives dvx/dvy/dw can be 0 if not available. + Returns motor commands: {FL, FR, BL, BR} clamped to motorOutputMax. */ + std::array update(float vx, float dvx, float vx_ref, + float vy, float dvy, float vy_ref, + float w, float dw, float w_ref); + +private: + // Simple default gains, will replace once robot has been simulated to get CARE-computed gains. + static Gains2 defaultGains(float dyn); + +private: + float m_; + float I_; + float maxOut_; + float scale_; + + Gains2 Kx_{}; + Gains2 Ky_{}; + Gains2 Kt_{}; +}; + +} // namespace chassis::algorithms +} // namespace control + +#endif // CHASSIS_LQR_HPP_ diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index 3cb07f1..20cf1fe 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -20,17 +20,42 @@ void ChassisSubsystem::initialize() backLeftMotor.initialize(); backRightMotor.initialize(); prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); + prevControlUpdate = prevRampUpdate; } void ChassisSubsystem::refresh() { updateRpmSetpoints(); - - uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevPidUpdate; - updateRpmPid(&frontLeftPid, &frontLeftMotor, frontLeftDesiredRpm, dt); - updateRpmPid(&frontRightPid, &frontRightMotor, frontRightDesiredRpm, dt); - updateRpmPid(&backLeftPid, &backLeftMotor, backLeftDesiredRpm, dt); - updateRpmPid(&backRightPid, &backRightMotor, backRightDesiredRpm, dt); - prevPidUpdate = tap::arch::clock::getTimeMilliseconds(); + // Ancien code PID (a conserver au cas ou) + // uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevPidUpdate; + // updateRpmPid(&frontLeftPid, &frontLeftMotor, frontLeftDesiredRpm, dt); + // updateRpmPid(&frontRightPid, &frontRightMotor, frontRightDesiredRpm, dt); + // updateRpmPid(&backLeftPid, &backLeftMotor, backLeftDesiredRpm, dt); + // updateRpmPid(&backRightPid, &backRightMotor, backRightDesiredRpm, dt); + // prevPidUpdate = tap::arch::clock::getTimeMilliseconds(); + + uint32_t now = tap::arch::clock::getTimeMilliseconds(); + float dt = (now - prevControlUpdate) / 1000.0f; + prevControlUpdate = now; + + float fl = static_cast(frontLeftMotor.getShaftRPM()); + float fr = static_cast(frontRightMotor.getShaftRPM()); + float bl = static_cast(backLeftMotor.getShaftRPM()); + float br = static_cast(backRightMotor.getShaftRPM()); + + float vx=0.f, vy=0.f, w=0.f; + rpmToBody(fl, fr, bl, br, vx, vy, w); + + // If we don't have estimated derivatives, we pass 0.f for dvx/dvy/dw + auto cmd = lqrController->update( + vx, 0.f, vxRef, + vy, 0.f, vyRef, + w, 0.f, wRef + ); + + frontLeftMotor.setDesiredOutput(cmd[0]); + frontRightMotor.setDesiredOutput(cmd[1]); + backLeftMotor.setDesiredOutput(cmd[2]); + backRightMotor.setDesiredOutput(cmd[3]); // Attempt to send a UART positionMessage to Jetson if the delay has elapsed if (tap::arch::clock::getTimeMilliseconds() - prevCVUpdate > CHASSIS_CV_UPDATE_PERIOD ) { @@ -47,35 +72,35 @@ void ChassisSubsystem::refresh() { // Front right debug message int nBytes = sprintf (buffer, "FR-RPM: %i, SETPOINT: %i\n", frontRightMotor.getShaftRPM(), - (int)frontRightDesiredRpm); + (int)cmd[0]); drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); // Front left debug message nBytes = sprintf (buffer, "FL-RPM: %i, SETPOINT: %i\n", frontLeftMotor.getShaftRPM(), - (int)frontLeftDesiredRpm); + (int)cmd[1]); drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); // Back right debug message nBytes = sprintf (buffer, "BR-RPM: %i, SETPOINT: %i\n", backRightMotor.getShaftRPM(), - (int)backRightDesiredRpm); + (int)cmd[2]); drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); // Back left debug message nBytes = sprintf (buffer, "BL-RPM: %i, SETPOINT: %i\n", backLeftMotor.getShaftRPM(), - (int)backLeftDesiredRpm); + (int)cmd[3]); drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); } } -void ChassisSubsystem::updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt) { - int64_t error = desiredRpm - motor->getShaftRPM(); - pid->runControllerDerivateError(error, dt); - if (desiredRpm == 0) { - motor->setDesiredOutput(0); - } else { - motor->setDesiredOutput(pid->getOutput()); - } -} +// void ChassisSubsystem::updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt) { +// int64_t error = desiredRpm - motor->getShaftRPM(); +// pid->runControllerDerivateError(error, dt); +// if (desiredRpm == 0) { +// motor->setDesiredOutput(0); +// } else { +// motor->setDesiredOutput(pid->getOutput()); +// } +// } void ChassisSubsystem::updateRpmSetpoints() { uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevRampUpdate; @@ -84,7 +109,11 @@ void ChassisSubsystem::updateRpmSetpoints() { if(yInputRamp.isTargetReached() == false) { yInputRamp.update(RAMP_SLOPE * dt); } if(rInputRamp.isTargetReached() == false) { rInputRamp.update(RAMP_SLOPE * dt); } - setDesiredOutput(xInputRamp.getValue(), yInputRamp.getValue(), rInputRamp.getValue()); + // setDesiredOutput(xInputRamp.getValue(), yInputRamp.getValue(), rInputRamp.getValue()); + vxRef = xInputRamp.getValue(); + vyRef = yInputRamp.getValue(); + wRef = rInputRamp.getValue(); + prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); } @@ -99,33 +128,33 @@ void ChassisSubsystem::setTargetOutput(float x, float y, float r) { +x is forward, +y is right, +r is clockwise (turning right). Expressed in body frame. */ -void ChassisSubsystem::setDesiredOutput(float x, float y, float r) -{ +// void ChassisSubsystem::setDesiredOutput(float x, float y, float r) +// { - x = tap::algorithms::limitVal(x,-1,1); - y = tap::algorithms::limitVal(y,-1,1); - r = tap::algorithms::limitVal(r,-1,1); +// x = tap::algorithms::limitVal(x,-1,1); +// y = tap::algorithms::limitVal(y,-1,1); +// r = tap::algorithms::limitVal(r,-1,1); - // x, y, and r contained between -1 and 1 - // Normalize movement vector - float norm = sqrt(x*x+y*y); - if (norm > 1) { - x = x / norm; - y = y / norm; - } - - y = IS_Y_INVERTED ? -y : y; - - switch (wheelType) { - case WheelType::omniwheel: - setOmniwheelDesiredRPM(x, y, r); - break; - case WheelType::mecanum: - default: - setMecanumDesiredRPM(x, y, r); - break; - } -} +// // x, y, and r contained between -1 and 1 +// // Normalize movement vector +// float norm = sqrt(x*x+y*y); +// if (norm > 1) { +// x = x / norm; +// y = y / norm; +// } + +// y = IS_Y_INVERTED ? -y : y; + +// switch (wheelType) { +// case WheelType::omniwheel: +// setOmniwheelDesiredRPM(x, y, r); +// break; +// case WheelType::mecanum: +// default: +// setMecanumDesiredRPM(x, y, r); +// break; +// } +// } /* Attempts to send IMU and wheel encoder data to CV over UART. @@ -195,19 +224,21 @@ void ChassisSubsystem::sendCVUpdate() { drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&positionMessage), sizeof(positionMessage)); } -void ChassisSubsystem::setMecanumDesiredRPM(const float& x, const float& y, const float& r) { - frontLeftDesiredRpm = (x-y-r)*rpmScaleFactor; - frontRightDesiredRpm = (x+y+r)*rpmScaleFactor; - backLeftDesiredRpm = (x+y-r)*rpmScaleFactor; - backRightDesiredRpm = (x-y+r)*rpmScaleFactor; -} - -void ChassisSubsystem::setOmniwheelDesiredRPM(const float& x, const float& y, const float& r) { - frontLeftDesiredRpm = (y+r) * rpmScaleFactor; - frontRightDesiredRpm = (-x-r) * rpmScaleFactor; - backLeftDesiredRpm = (-x+r) * rpmScaleFactor; - backRightDesiredRpm = (y-r) * rpmScaleFactor; -} +// Old methods to set wheel RPMs based on desired x, y, r inputs with PID control + +// void ChassisSubsystem::setMecanumDesiredRPM(const float& x, const float& y, const float& r) { +// frontLeftDesiredRpm = (x-y-r)*rpmScaleFactor; +// frontRightDesiredRpm = (x+y+r)*rpmScaleFactor; +// backLeftDesiredRpm = (x+y-r)*rpmScaleFactor; +// backRightDesiredRpm = (x-y+r)*rpmScaleFactor; +// } + +// void ChassisSubsystem::setOmniwheelDesiredRPM(const float& x, const float& y, const float& r) { +// frontLeftDesiredRpm = (y+r) * rpmScaleFactor; +// frontRightDesiredRpm = (-x-r) * rpmScaleFactor; +// backLeftDesiredRpm = (-x+r) * rpmScaleFactor; +// backRightDesiredRpm = (y-r) * rpmScaleFactor; +// } } // namespace chassis diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index 58a7154..d010920 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -1,15 +1,17 @@ #ifndef CHASSIS_SUBSYSTEM_HPP_ #define CHASSIS_SUBSYSTEM_HPP_ +#include #include "tap/control/subsystem.hpp" -#include "tap/algorithms/smooth_pid.hpp" -#include "modm/math/filter/pid.hpp" +// #include "tap/algorithms/smooth_pid.hpp" +// #include "modm/math/filter/pid.hpp" #include "tap/algorithms/ramp.hpp" #include "tap/motor/dji_motor.hpp" #include "tap/util_macros.hpp" #include "chassis_constants.hpp" #include "control/drivers/drivers.hpp" #include "control/robot_config.hpp" +#include "algorithms/chassis_lqr.hpp" //#include "control/control_operator_interface_edu.hpp" @@ -17,6 +19,7 @@ namespace control { namespace chassis { + /** * A bare bones Subsystem for interacting with a 4 wheeled chassis. */ @@ -46,16 +49,17 @@ class ChassisSubsystem : public tap::control::Subsystem frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "back right motor"), - frontLeftPid(pidConfig), - frontRightPid(pidConfig), - backLeftPid(pidConfig), - backRightPid(pidConfig), - frontLeftDesiredRpm(0), - frontRightDesiredRpm(0), - backLeftDesiredRpm(0), - backRightDesiredRpm(0), + // frontLeftPid(pidConfig), + // frontRightPid(pidConfig), + // backLeftPid(pidConfig), + // backRightPid(pidConfig), + // frontLeftDesiredRpm(0), + // frontRightDesiredRpm(0), + // backLeftDesiredRpm(0), + // backRightDesiredRpm(0), prevCVUpdate(0) { + lqrController = std::make_unique(); } ChassisSubsystem(const ChassisSubsystem &other) = delete; @@ -70,7 +74,7 @@ class ChassisSubsystem : public tap::control::Subsystem void setDesiredOutput(float x, float y, float r); - void updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt); + // void updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt); void updateRpmSetpoints(); void setTargetOutput(float x, float y, float r); @@ -81,6 +85,18 @@ class ChassisSubsystem : public tap::control::Subsystem const tap::motor::DjiMotor &getBackLeftMotor() const { return backLeftMotor; } const tap::motor::DjiMotor &getBackRightMotor() const { return backRightMotor; } + inline void rpmToBody(float fl, float fr, float bl, float br, + float& xNorm, float& yNorm, float& rNorm) const + { + constexpr float S = rpmScaleFactor; + // Avoid divide-by-zero if someone changes S later + if (S == 0.0f) { xNorm = yNorm = rNorm = 0.0f; return; } + + yNorm = (fl + br) / (2.0f * S); + rNorm = (fl - br) / (2.0f * S); + xNorm = -(fr + bl) / (2.0f * S); + } + private: void setMecanumDesiredRPM(const float& x, const float& y, const float& r); void setOmniwheelDesiredRPM(const float& x, const float& y, const float& r); @@ -101,23 +117,33 @@ class ChassisSubsystem : public tap::control::Subsystem tap::motor::DjiMotor backLeftMotor; tap::motor::DjiMotor backRightMotor; - // Smooth PID configuration - tap::algorithms::SmoothPidConfig pidConfig = { CHASSIS_PID_KP, CHASSIS_PID_KI, CHASSIS_PID_KD, - CHASSIS_PID_MAX_ERROR_SUM, CHASSIS_PID_MAX_OUTPUT, - CHASSIS_TQ_DERIVATIVE_KALMAN, CHASSIS_TR_DERIVATIVE_KALMAN, - CHASSIS_TQ_PROPORTIONAL_KALMAN, CHASSIS_TR_PROPORTIONAL_KALMAN }; + // LQR controller for chassis + std::unique_ptr lqrController; + + // // Smooth PID configuration + // tap::algorithms::SmoothPidConfig pidConfig = { CHASSIS_PID_KP, CHASSIS_PID_KI, CHASSIS_PID_KD, + // CHASSIS_PID_MAX_ERROR_SUM, CHASSIS_PID_MAX_OUTPUT, + // CHASSIS_TQ_DERIVATIVE_KALMAN, CHASSIS_TR_DERIVATIVE_KALMAN, + // CHASSIS_TQ_PROPORTIONAL_KALMAN, CHASSIS_TR_PROPORTIONAL_KALMAN }; - // Smooth PID controllers for position feedback from motors - tap::algorithms::SmoothPid frontLeftPid; - tap::algorithms::SmoothPid frontRightPid; - tap::algorithms::SmoothPid backLeftPid; - tap::algorithms::SmoothPid backRightPid; - - ///< Any user input is translated into desired RPM for each motor. - float frontLeftDesiredRpm; - float frontRightDesiredRpm; - float backLeftDesiredRpm; - float backRightDesiredRpm; + // // Smooth PID controllers for position feedback from motors + // tap::algorithms::SmoothPid frontLeftPid; + // tap::algorithms::SmoothPid frontRightPid; + // tap::algorithms::SmoothPid backLeftPid; + // tap::algorithms::SmoothPid backRightPid; + + float vxRef = 0.0f; // desired forward speed in [-1, 1] + float vyRef = 0.0f; // desired rightward speed in [-1, 1] + float wRef = 0.0f; // desired CW rotation in [-1, 1] + + //] Previous time the LQR control loop was updated + uint32_t prevControlUpdate = 0; + + // ///< Any user input is translated into desired RPM for each motor. + // float frontLeftDesiredRpm; + // float frontRightDesiredRpm; + // float backLeftDesiredRpm; + // float backRightDesiredRpm; // Ramp for each input tap::algorithms::Ramp xInputRamp; @@ -137,7 +163,7 @@ class ChassisSubsystem : public tap::control::Subsystem static constexpr float rpmScaleFactor = 3500.0f; uint32_t prevDebugTime; - uint32_t prevPidUpdate; + // uint32_t prevPidUpdate; // Variables for managing UART messages sent to CV uint32_t prevCVUpdate; diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp new file mode 100644 index 0000000..1428d51 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -0,0 +1,40 @@ +#include "turret_lqr.hpp" +#include + +namespace turret::algorithms +{ +TurretLqrController::TurretLqrController(float panInertia, float tiltInertia, + float Qscale, float Rscale) + : I_pan(panInertia), + I_tilt(tiltInertia) +{ + K_pan = computeLqrGain(I_pan, Qscale, Rscale); + K_tilt = computeLqrGain(I_tilt, Qscale, Rscale); +} + +// Called periodically to update the controller state +void TurretLqrController::update(float panAngle, float panRate, float panTarget, + float tiltAngle, float tiltRate, float tiltTarget) +{ + float panError = panAngle - panTarget; + float tiltError = tiltAngle - tiltTarget; + + panVoltage = -(K_pan[0] * panError + K_pan[1] * panRate); // Power adjustment to add/remove for x axis + tiltVoltage = -(K_tilt[0] * tiltError + K_tilt[1] * tiltRate); // Power adjustment to add/remove for y axis +} + +float TurretLqrController::getPanVoltage() const { return panVoltage; } +float TurretLqrController::getTiltVoltage() const { return tiltVoltage; } + +// Model per axis: A=[0 1; 0 0], B=[0; 1/I], Q=diag(q,q), R=r +std::array TurretLqrController::computeLqrGain(float inertia, float Qscale, float Rscale) +{ + float q = Qscale; + float r = Rscale; + + float k_angle = std::sqrt(q / r) * inertia; + float k_rate = std::sqrt(q * r); + + return { k_angle, k_rate }; +} +} // namespace turret::algorithms diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp new file mode 100644 index 0000000..1faa7ef --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp @@ -0,0 +1,39 @@ +#ifndef TURRET_LQR_CONTROLLER_HPP_ +#define TURRET_LQR_CONTROLLER_HPP_ + +#include + +namespace turret::algorithms +{ +/* LQR controller for a 2-DOF turret (pan, tilt). + Regulates angle/rate on each axis and outputs a motor command. */ +class TurretLqrController +{ +public: + TurretLqrController(float panInertia, float tiltInertia, + float Qscale = 1.0f, float Rscale = 1.0f); + + // Update the controller and compute outputs for pan/tilt (x,y axes). + void update(float panAngle, float panRate, float panTarget, + float tiltAngle, float tiltRate, float tiltTarget); + + float getPanVoltage() const; + float getTiltVoltage() const; + +private: + float I_pan; + float I_tilt; + + // K = [k_angle, k_rate] for each axis (u = -K * [angle_error, rate]) + std::array K_pan; + std::array K_tilt; + + float panVoltage = 0.f; + float tiltVoltage = 0.f; + + // Simple initializer for testing, will replace with CARE-based gains when computed + std::array computeLqrGain(float inertia, float Qscale, float Rscale); +}; +} // namespace turret::algorithms + +#endif // TURRET_LQR__HPP_ diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp index 71fbae9..694f194 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp @@ -46,10 +46,21 @@ static constexpr uint32_t TURRET_CV_UPDATE_PERIOD = 10; */ static constexpr float RPM_TO_DEGPERMS = 0.006; static constexpr float DEGREE_TO_MILLIRAD = 17.453293; +static constexpr float DEG_TO_RAD = DEGREE_TO_MILLIRAD / 1000.0f; +static constexpr float RPM_TO_RAD_S = RPM_TO_DEGPERMS * DEGREE_TO_MILLIRAD; + /** * Spin2win stabilization constants * Represents how much the joystick should have move per ms to stabilize the turret */ static constexpr float LOW_ROTATION = 0.67; -static constexpr float HIGH_ROTATION = 0.95; \ No newline at end of file +static constexpr float HIGH_ROTATION = 0.95; + +/** + * LQR parameters (need to be computed with Simulink or similar CARE solver if you want to change them) + */ +static constexpr float TURRET_PAN_INERTIA = 0.0051f; // kg·m² +static constexpr float TURRET_TILT_INERTIA = 0.0015f; // kg·m² +static constexpr float TURRET_LQR_Q = 50.0f; +static constexpr float TURRET_LQR_R = 0.05f; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index 293670e..34196da 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -68,9 +68,17 @@ void TurretSubsystem::runYawController(uint32_t dt) { } int16_t currentRPM = yawMotor->getShaftRPM(); - cascadedYawController.update(error, currentRPM, dt); + float errDeg = DjiMotor::encoderToDegrees((int64_t)error); + float errRad = errDeg * DEG_TO_RAD; + float omega = static_cast(currentRPM) * RPM_TO_RAD_S; - yawMotor->setDesiredOutput(cascadedYawController.getOutput()); + lqrTurret.update(/*panAngle*/ errRad, /*panRate*/ omega, /*panTarget*/ 0.0f, + /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltTarget*/ 0.0f); + yawMotor->setDesiredOutput(lqrTurret.getPanVoltage()); + + // cascadedYawController.update(error, currentRPM, dt); + + // yawMotor->setDesiredOutput(cascadedYawController.getOutput()); } /* @@ -80,9 +88,16 @@ void TurretSubsystem::runPitchController(uint32_t dt) { float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); int16_t currentRPM = pitchMotor.getShaftRPM(); - cascadedPitchController.update(error, currentRPM, dt); + float errDeg = pitchMotor.encoderToDegrees((int64_t)error); + float errRad = errDeg * DEG_TO_RAD; + float omega = static_cast(currentRPM) * RPM_TO_RAD_S; + + lqrTurret.update(/*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panTarget*/ 0.0f, + /*tiltAngle*/ errRad, /*tiltRate*/ omega, /*tiltTarget*/ 0.0f); + pitchMotor.setDesiredOutput(lqrTurret.getTiltVoltage()); + // cascadedPitchController.update(error, currentRPM, dt); - pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); + // pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); } /* @@ -167,59 +182,59 @@ void TurretSubsystem::sendDebugInfo(bool sendYaw, bool sendPitch) { /* Velocity Control debug information, used during tuning of the inner loops. */ -void TurretSubsystem::sendTuningDebugInfo(bool sendYaw, bool sendPitch, float velSetpoint, float threshold) { - char buffer[500]; +// void TurretSubsystem::sendTuningDebugInfo(bool sendYaw, bool sendPitch, float velSetpoint, float threshold) { +// char buffer[500]; - int nBytes; - - if (sendYaw) { - float error = yawDesiredPos - yawMotor->getEncoderWrapped(); - if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { - error = error - DjiMotor::ENC_RESOLUTION * getSign(error); - } - float yawDesiredVel = error > threshold ? velSetpoint : error < -threshold ? -velSetpoint : 0; - nBytes = sprintf (buffer, "Yaw RPM: %i, Setpoint: %i\n", - (int)(yawMotor->getShaftRPM()), - (int)(yawDesiredVel)); - drivers->uart.write(TURRET_DEBUG_PORT,(uint8_t*) buffer, nBytes+1); - } - - if (sendPitch) { - float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); - float pitchDesiredVel = error > threshold ? velSetpoint : error < -threshold ? -velSetpoint : 0; - nBytes = sprintf (buffer, "Pitch RPM: %i, Setpoint: %i\n", - (int)(pitchMotor.getShaftRPM()), - (int)(pitchDesiredVel)); - drivers->uart.write(TURRET_DEBUG_PORT,(uint8_t*) buffer, nBytes+1); - } -} +// int nBytes; + +// if (sendYaw) { +// float error = yawDesiredPos - yawMotor->getEncoderWrapped(); +// if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { +// error = error - DjiMotor::ENC_RESOLUTION * getSign(error); +// } +// float yawDesiredVel = error > threshold ? velSetpoint : error < -threshold ? -velSetpoint : 0; +// nBytes = sprintf (buffer, "Yaw RPM: %i, Setpoint: %i\n", +// (int)(yawMotor->getShaftRPM()), +// (int)(yawDesiredVel)); +// drivers->uart.write(TURRET_DEBUG_PORT,(uint8_t*) buffer, nBytes+1); +// } + +// if (sendPitch) { +// float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); +// float pitchDesiredVel = error > threshold ? velSetpoint : error < -threshold ? -velSetpoint : 0; +// nBytes = sprintf (buffer, "Pitch RPM: %i, Setpoint: %i\n", +// (int)(pitchMotor.getShaftRPM()), +// (int)(pitchDesiredVel)); +// drivers->uart.write(TURRET_DEBUG_PORT,(uint8_t*) buffer, nBytes+1); +// } +// } /* Run yaw inner loop. Used when tuning. */ -void TurretSubsystem::yawInnerLoopTest(uint32_t dt, float velSetpoint, float threshold) { - int64_t error = yawDesiredPos - yawMotor->getEncoderWrapped(); - if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { - error = error - DjiMotor::ENC_RESOLUTION * getSign(error); - } - int16_t currentRPM = yawMotor->getShaftRPM(); +// void TurretSubsystem::yawInnerLoopTest(uint32_t dt, float velSetpoint, float threshold) { +// int64_t error = yawDesiredPos - yawMotor->getEncoderWrapped(); +// if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { +// error = error - DjiMotor::ENC_RESOLUTION * getSign(error); +// } +// int16_t currentRPM = yawMotor->getShaftRPM(); - cascadedYawController.testInnerLoop(error, currentRPM, dt, velSetpoint, threshold); +// cascadedYawController.testInnerLoop(error, currentRPM, dt, velSetpoint, threshold); - yawMotor->setDesiredOutput(cascadedYawController.getOutput()); -} +// yawMotor->setDesiredOutput(cascadedYawController.getOutput()); +// } /* Run pitch inner loop. Used when tuning. */ -void TurretSubsystem::pitchInnerLoopTest(uint32_t dt, float velSetpoint, float threshold) { - float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); - int16_t currentRPM = pitchMotor.getShaftRPM(); +// void TurretSubsystem::pitchInnerLoopTest(uint32_t dt, float velSetpoint, float threshold) { +// float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); +// int16_t currentRPM = pitchMotor.getShaftRPM(); - cascadedPitchController.testInnerLoop(error, currentRPM, dt, velSetpoint, threshold); +// cascadedPitchController.testInnerLoop(error, currentRPM, dt, velSetpoint, threshold); - pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); -} +// pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); +// } } // namespace turret diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp index 856f918..9dde172 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp @@ -6,9 +6,11 @@ #include "tap/util_macros.hpp" #include "control/drivers/drivers.hpp" #include "turret_constants.hpp" -#include "algorithms/cascaded_pid.hpp" +// #include "algorithms/cascaded_pid.hpp" // old PID +#include "algorithms/turret_lqr.hpp" -using turret::algorithms::CascadedPid; +// using turret::algorithms::CascadedPid; +using turret::algorithms::TurretLqrController; namespace control { @@ -31,8 +33,9 @@ class TurretSubsystem : public tap::control::Subsystem : tap::control::Subsystem(drivers), yawMotor(yawMotor), pitchMotor(drivers, PITCH_MOTOR_ID, CAN_BUS_MOTORS, PITCH_IS_INVERTED, "pitch motor"), - cascadedPitchController(PITCH_OUTER_PID_CONFIG, PITCH_INNER_PID_CONFIG), - cascadedYawController(YAW_OUTER_PID_CONFIG, YAW_INNER_PID_CONFIG), + // cascadedPitchController(PITCH_OUTER_PID_CONFIG, PITCH_INNER_PID_CONFIG), + // cascadedYawController(YAW_OUTER_PID_CONFIG, YAW_INNER_PID_CONFIG), + lqrTurret(TURRET_PAN_INERTIA, TURRET_TILT_INERTIA, /*Q*/TURRET_LQR_Q, /*R*/TURRET_LQR_R), yawDesiredPos(YAW_NEUTRAL_POS), pitchDesiredPos(PITCH_NEUTRAL_POS) { @@ -73,10 +76,10 @@ class TurretSubsystem : public tap::control::Subsystem void sendCVUpdate(); void sendDebugInfo(bool sendYaw, bool sendPitch); - // Methods used when tuning the inner loop of the cascaded PID controller - void yawInnerLoopTest(uint32_t dt, float velSetpoint, float threshold); - void pitchInnerLoopTest(uint32_t dt, float velSetpoint, float threshold); - void sendTuningDebugInfo(bool sendYaw, bool sendPitch, float velSetpoint, float threshold); + // // Methods used when tuning the inner loop of the cascaded PID controller + // void yawInnerLoopTest(uint32_t dt, float velSetpoint, float threshold); + // void pitchInnerLoopTest(uint32_t dt, float velSetpoint, float threshold); + // void sendTuningDebugInfo(bool sendYaw, bool sendPitch, float velSetpoint, float threshold); // Hardware constants static constexpr tap::motor::MotorId YAW_MOTOR_ID = tap::motor::MOTOR6; @@ -88,9 +91,10 @@ class TurretSubsystem : public tap::control::Subsystem tap::motor::DjiMotor *yawMotor; tap::motor::DjiMotor pitchMotor; - // Motor Controllers for position control - CascadedPid cascadedPitchController; - CascadedPid cascadedYawController; + // // Motor Controllers for position control + // CascadedPid cascadedPitchController; + // CascadedPid cascadedYawController; + TurretLqrController lqrTurret; // Position setpoints for turret, in encoder ticks float yawDesiredPos; From 0f647d7c35224bd6cad0320310ccfaa6b738ab9e Mon Sep 17 00:00:00 2001 From: antoinefischer Date: Sat, 11 Oct 2025 15:44:49 -0400 Subject: [PATCH 03/12] Builds and runs, though axis is weird and turn input not implemented (spinning works though) --- .vscode/c_cpp_properties.json | 100 +++++---- .../subsystems/chassis/chassis_constants.hpp | 3 + .../chassis/chassis_spin2win_subsystem.cpp | 200 ++++++++++++++++++ .../chassis/chassis_spin2win_subsystem.hpp | 149 +++++++++++++ .../subsystems/chassis/chassis_subsystem.cpp | 38 ++-- .../subsystems/chassis/chassis_subsystem.hpp | 10 +- 6 files changed, 431 insertions(+), 69 deletions(-) create mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp create mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 1949137..c5d0964 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -1,42 +1,58 @@ -{ - "configurations": [ - { - "name": "Test", - "defines": ["${default}", "ENV_UNIT_TESTS", "PLATFORM_HOSTED", "RUN_WITH_PROFILING"], - "includePath": [ - "${default}", - "PolySTAR-Taproot-project/taproot/sim-modm/hosted-linux/modm/src", - "PolySTAR-Taproot-project/taproot/test", - "PolySTAR-Taproot-project/test" - ], - "compilerPath": "g++", - "intelliSenseMode": "gcc-x64", - "browse": { "limitSymbolsToIncludedHeaders": true } - }, - { - "name": "Sim", - "defines": ["${default}", "PLATFORM_HOSTED", "RUN_WITH_PROFILING"], - "includePath": [ - "${default}", - "PolySTAR-Taproot-project/taproot/sim-modm/hosted-linux/modm/src" - ], - "compilerPath": "g++", - "intelliSenseMode": "gcc-x64", - "browse": { "limitSymbolsToIncludedHeaders": true } - }, - { - "name": "Hardware", - "includePath": [ - "${default}", - "PolySTAR-Taproot-project/taproot/modm/src", - "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/core", - "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/device", - "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/dsp" - ], - "compilerPath": "arm-none-eabi-g++", - "intelliSenseMode": "gcc-arm", - "browse": { "limitSymbolsToIncludedHeaders": true } - } - ], - "version": 4 -} +{ + "configurations": [ + { + "name": "Test", + "defines": [ + "${default}", + "ENV_UNIT_TESTS", + "PLATFORM_HOSTED", + "RUN_WITH_PROFILING" + ], + "includePath": [ + "${default}", + "PolySTAR-Taproot-project/taproot/sim-modm/hosted-linux/modm/src", + "PolySTAR-Taproot-project/taproot/test", + "PolySTAR-Taproot-project/test" + ], + "compilerPath": "g++", + "intelliSenseMode": "gcc-x64", + "browse": { + "limitSymbolsToIncludedHeaders": true + } + }, + { + "name": "Sim", + "defines": [ + "${default}", + "PLATFORM_HOSTED", + "RUN_WITH_PROFILING" + ], + "includePath": [ + "${default}", + "PolySTAR-Taproot-project/taproot/sim-modm/hosted-linux/modm/src" + ], + "compilerPath": "g++", + "intelliSenseMode": "gcc-x64", + "browse": { + "limitSymbolsToIncludedHeaders": true + } + }, + { + "name": "Hardware", + "includePath": [ + "${default}", + "PolySTAR-Taproot-project/taproot/modm/src", + "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/core", + "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/device", + "PolySTAR-Taproot-project/taproot/modm/ext/cmsis/dsp", + "${workspaceFolder}/PolySTAR-Taproot-project/src/subsystems/chassis" + ], + "compilerPath": "arm-none-eabi-g++", + "intelliSenseMode": "gcc-arm", + "browse": { + "limitSymbolsToIncludedHeaders": true + } + } + ], + "version": 4 +} \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp index 337f7e9..60ca53a 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp @@ -19,6 +19,9 @@ static constexpr float CHASSIS_TR_PROPORTIONAL_KALMAN = 0.0f; * of the chassis when using the keyboard. */ +static constexpr float CHASSIS_MASS_KG = 17.2597f; +static constexpr float CHASSIS_YAW_INERTIA_KGM2 = 1.139542f; + #ifdef TARGET_HERO static constexpr float CHASSIS_DEFAULT_SPEED = 0.25f; diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp new file mode 100644 index 0000000..b6c939e --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp @@ -0,0 +1,200 @@ +#include "chassis_spin2win_subsystem.hpp" + +#include "tap/communication/serial/remote.hpp" +#include "tap/algorithms/math_user_utils.hpp" +#include "control/drivers/drivers.hpp" +#include "communication/cv_handler.hpp" + +using namespace tap; +using tap::communication::serial::Uart; + +namespace control +{ +namespace chassis +{ + +void ChassisSpin2WinSubsystem::initialize() +{ + frontLeftMotor.initialize(); + frontRightMotor.initialize(); + backLeftMotor.initialize(); + backRightMotor.initialize(); + prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); +} + +void ChassisSpin2WinSubsystem::refresh() { + updateRpmSetpoints(); + + uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevPidUpdate; + updateRpmPid(&frontLeftPid, &frontLeftMotor, frontLeftDesiredRpm, dt); + updateRpmPid(&frontRightPid, &frontRightMotor, frontRightDesiredRpm, dt); + updateRpmPid(&backLeftPid, &backLeftMotor, backLeftDesiredRpm, dt); + updateRpmPid(&backRightPid, &backRightMotor, backRightDesiredRpm, dt); + prevPidUpdate = tap::arch::clock::getTimeMilliseconds(); + + // Attempt to send a UART positionMessage to Jetson if the delay has elapsed + if (tap::arch::clock::getTimeMilliseconds() - prevCVUpdate > CHASSIS_CV_UPDATE_PERIOD ) { + prevCVUpdate = tap::arch::clock::getTimeMilliseconds(); + sendCVUpdate(); + } + + if (CHASSIS_DEBUG_MESSAGE == false) return; + + if (tap::arch::clock::getTimeMilliseconds() - prevDebugTime > CHASSIS_DEBUG_MESSAGE_DELAY_MS) { + prevDebugTime = tap::arch::clock::getTimeMilliseconds(); + char buffer[500]; + + // Front right debug message + int nBytes = sprintf (buffer, "FR-RPM: %i, SETPOINT: %i\n", + frontRightMotor.getShaftRPM(), + (int)frontRightDesiredRpm); + drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); + // Front left debug message + nBytes = sprintf (buffer, "FL-RPM: %i, SETPOINT: %i\n", + frontLeftMotor.getShaftRPM(), + (int)frontLeftDesiredRpm); + drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); + // Back right debug message + nBytes = sprintf (buffer, "BR-RPM: %i, SETPOINT: %i\n", + backRightMotor.getShaftRPM(), + (int)backRightDesiredRpm); + drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); + // Back left debug message + nBytes = sprintf (buffer, "BL-RPM: %i, SETPOINT: %i\n", + backLeftMotor.getShaftRPM(), + (int)backLeftDesiredRpm); + drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); + } +} + +void ChassisSpin2WinSubsystem::updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt) { + int64_t error = desiredRpm - motor->getShaftRPM(); + pid->runControllerDerivateError(error, dt); + if (desiredRpm == 0) { + motor->setDesiredOutput(0); + } else { + motor->setDesiredOutput(pid->getOutput()); + } +} + +void ChassisSpin2WinSubsystem::updateRpmSetpoints() { + uint32_t dt = tap::arch::clock::getTimeMilliseconds() - prevRampUpdate; + + if(xInputRamp.isTargetReached() == false) { xInputRamp.update(RAMP_SLOPE * dt); } + if(yInputRamp.isTargetReached() == false) { yInputRamp.update(RAMP_SLOPE * dt); } + if(rInputRamp.isTargetReached() == false) { rInputRamp.update(RAMP_SLOPE * dt); } + + setDesiredOutput(xInputRamp.getValue(), yInputRamp.getValue(), rInputRamp.getValue()); + prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); +} + +void ChassisSpin2WinSubsystem::setTargetOutput(float x, float y, float r) { + xInputRamp.setTarget(x); + yInputRamp.setTarget(y); + rInputRamp.setTarget(r); +} + +/* + Give desired setpoints for chassis movement. + +x is forward, +y is right, +r is clockwise (turning right). + Expressed in body frame. +*/ +void ChassisSpin2WinSubsystem::setDesiredOutput(float x, float y, float r) +{ + + x = tap::algorithms::limitVal(x,-1,1); + y = tap::algorithms::limitVal(y,-1,1); + r = tap::algorithms::limitVal(r,-1,1); + + // x, y, and r contained between -1 and 1 + // Normalize movement vector + float norm = sqrt(x*x+y*y); + if (norm > 1) { + x = x / norm; + y = y / norm; + } + + y = IS_Y_INVERTED ? -y : y; + + float frontLeftValue = y + r; + float frontRightValue = -x - r; + float backLeftValue = -x + r; + float backRightValue = y - r; + + frontLeftDesiredRpm = frontLeftValue * rpmScaleFactor; + frontRightDesiredRpm = frontRightValue * rpmScaleFactor; + backLeftDesiredRpm = backLeftValue * rpmScaleFactor; + backRightDesiredRpm = backRightValue * rpmScaleFactor; +} + +/* + Attempts to send IMU and wheel encoder data to CV over UART. + Returns true if the positionMessage was sent sucessfully. +*/ +void ChassisSpin2WinSubsystem::sendCVUpdate() { + + // Get IMU measurements + float Ax = drivers->mpu6500.getAx(); + float Ay = drivers->mpu6500.getAy(); + float Az = drivers->mpu6500.getAz(); + float Gx = drivers->mpu6500.getGx(); + float Gy = drivers->mpu6500.getGy(); + float Gz = drivers->mpu6500.getGz(); + float Rx = drivers->mpu6500.getRoll(); + float Ry = drivers->mpu6500.getPitch(); + float Rz = drivers->mpu6500.getYaw(); + + // Get motor encoder positions + // Revolutions is calculated because DJIMotor interface does not have the getter for this value + uint16_t frontLeftEncoder = frontLeftMotor.getEncoderWrapped(); + int16_t frontLeftRevolutions = (frontLeftMotor.getEncoderUnwrapped() - frontLeftEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; + uint16_t frontRightEncoder = frontRightMotor.getEncoderWrapped(); + int16_t frontRightRevolutions = (frontRightMotor.getEncoderUnwrapped() - frontRightEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; + uint16_t backLeftEncoder = backLeftMotor.getEncoderWrapped(); + int16_t backLeftRevolutions = (backLeftMotor.getEncoderUnwrapped() - backLeftEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; + uint16_t backRightEncoder = backRightMotor.getEncoderWrapped(); + int16_t backRightRevolutions = (backRightMotor.getEncoderUnwrapped() - backRightEncoder)/tap::motor::DjiMotor::ENC_RESOLUTION; + + // Get motor RPMs + int16_t frontLeftRPM = frontLeftMotor.getShaftRPM(); + int16_t frontRightRPM = frontRightMotor.getShaftRPM(); + int16_t backLeftRPM = backLeftMotor.getShaftRPM(); + int16_t backRightRPM = backRightMotor.getShaftRPM(); + + // Convert IMU and encoder data to 2 byte data types for transmission + // Conversions need to occur to respect 2 byte limit for each value sent + // Accelerations : converted from m/s2 to int16_t mm/s2 + // Gyro : converted from deg/s to int16_t millirad/s + // Attitude : converted from deg to int16_t millirad + // Encoder positions: passed as is + // Encoder revolutions: converted to int16_t + // Encoder RPM: passed as is + src::communication::cv::CVSerialData::Tx::PositionMessage positionMessage; + positionMessage.Ax = static_cast(Ax*M_TO_MM); + positionMessage.Ay = static_cast(Ay*M_TO_MM); + positionMessage.Az = static_cast(Az*M_TO_MM); + positionMessage.Gx = static_cast(Gx*DEG_TO_MILLIRAD); + positionMessage.Gy = static_cast(Gy*DEG_TO_MILLIRAD); + positionMessage.Gz = static_cast(Gz*DEG_TO_MILLIRAD); + positionMessage.Rx = static_cast(Rx*DEG_TO_MILLIRAD); + positionMessage.Ry = static_cast(Ry*DEG_TO_MILLIRAD); + positionMessage.Rz = static_cast(Rz*DEG_TO_MILLIRAD); + positionMessage.frontLeftEncoder = frontLeftEncoder; + positionMessage.frontLeftRevolutions = frontLeftRevolutions; + positionMessage.frontRightEncoder = frontRightEncoder; + positionMessage.frontRightRevolutions = frontRightRevolutions; + positionMessage.backLeftEncoder = backLeftEncoder; + positionMessage.backLeftRevolutions = backLeftRevolutions; + positionMessage.backRightEncoder = backRightEncoder; + positionMessage.backRightRevolutions = backRightRevolutions; + positionMessage.frontLeftRPM = frontLeftRPM; + positionMessage.frontRightRPM = frontRightRPM; + positionMessage.backLeftRPM = backLeftRPM; + positionMessage.backRightRPM = backRightRPM; + + drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&positionMessage), sizeof(positionMessage)); +} + +} // namespace chassis + +} // namespace control \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp new file mode 100644 index 0000000..bfd2d8a --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp @@ -0,0 +1,149 @@ +#ifndef CHASSIS_SPIN2WIN_SUBSYSTEM_HPP_ +#define CHASSIS_SPIN2WIN_SUBSYSTEM_HPP_ + +#include "tap/control/subsystem.hpp" +#include "tap/algorithms/smooth_pid.hpp" +#include "modm/math/filter/pid.hpp" +#include "tap/algorithms/ramp.hpp" +#include "tap/motor/dji_motor.hpp" +#include "tap/util_macros.hpp" +#include "chassis_constants.hpp" +#include "control/drivers/drivers.hpp" + +//#include "control/control_operator_interface_edu.hpp" + +namespace control +{ +namespace chassis +{ +/** + * A bare bones Subsystem for interacting with a 4 wheeled chassis. + */ +class ChassisSpin2WinSubsystem : public tap::control::Subsystem +{ +public: + /** + * This max output is measured in the c620 robomaster translated current. + * Per the datasheet, the controllable current range is -16384 ~ 0 ~ 16384. + * The corresponding speed controller output torque current range is + * -20 ~ 0 ~ 20 A. + * + * For this demo, we have capped the output at 8000. This should be more + * than enough for what you are doing. + */ + static constexpr float MAX_CURRENT_OUTPUT = 8000.0f; + + /** + * Constructs a new ChassisSpin2WinSubsystem with default parameters specified in + * the private section of this class. + */ + ChassisSpin2WinSubsystem(src::Drivers *drivers) + : tap::control::Subsystem(drivers), + drivers(drivers), + frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), + frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), + backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), + backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "back right motor"), + frontLeftPid(pidConfig), + frontRightPid(pidConfig), + backLeftPid(pidConfig), + backRightPid(pidConfig), + frontLeftDesiredRpm(0), + frontRightDesiredRpm(0), + backLeftDesiredRpm(0), + backRightDesiredRpm(0), + prevCVUpdate(0) + { + } + + ChassisSpin2WinSubsystem(const ChassisSpin2WinSubsystem &other) = delete; + + ChassisSpin2WinSubsystem &operator=(const ChassisSpin2WinSubsystem &other) = delete; + + ~ChassisSpin2WinSubsystem() = default; + + void initialize() override; + + void refresh() override; + + void setDesiredOutput(float x, float y, float r); + + void updateRpmPid(tap::algorithms::SmoothPid* pid, tap::motor::DjiMotor* const motor, float desiredRpm, uint32_t dt); + void updateRpmSetpoints(); + void setTargetOutput(float x, float y, float r); + + void sendCVUpdate(); + + const tap::motor::DjiMotor &getFrontLeftMotor() const { return frontLeftMotor; } + const tap::motor::DjiMotor &getFrontRightMotor() const { return frontRightMotor; } + const tap::motor::DjiMotor &getBackLeftMotor() const { return backLeftMotor; } + const tap::motor::DjiMotor &getBackRightMotor() const { return backRightMotor; } + +private: + src::Drivers *drivers; + + ///< Hardware constants, not specific to any particular chassis. + static constexpr tap::motor::MotorId FRONT_LEFT_MOTOR_ID = tap::motor::MOTOR1; + static constexpr tap::motor::MotorId FRONT_RIGHT_MOTOR_ID = tap::motor::MOTOR2; + static constexpr tap::motor::MotorId BACK_RIGHT_MOTOR_ID = tap::motor::MOTOR3; + static constexpr tap::motor::MotorId BACK_LEFT_MOTOR_ID = tap::motor::MOTOR4; + static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; + + ///< Motors. Use these to interact with any dji style motors. + tap::motor::DjiMotor frontLeftMotor; + tap::motor::DjiMotor frontRightMotor; + tap::motor::DjiMotor backLeftMotor; + tap::motor::DjiMotor backRightMotor; + + // Smooth PID configuration + tap::algorithms::SmoothPidConfig pidConfig = { CHASSIS_PID_KP, CHASSIS_PID_KI, CHASSIS_PID_KD, + CHASSIS_PID_MAX_ERROR_SUM, CHASSIS_PID_MAX_OUTPUT, + CHASSIS_TQ_DERIVATIVE_KALMAN, CHASSIS_TR_DERIVATIVE_KALMAN, + CHASSIS_TQ_PROPORTIONAL_KALMAN, CHASSIS_TR_PROPORTIONAL_KALMAN }; + + // Smooth PID controllers for position feedback from motors + tap::algorithms::SmoothPid frontLeftPid; + tap::algorithms::SmoothPid frontRightPid; + tap::algorithms::SmoothPid backLeftPid; + tap::algorithms::SmoothPid backRightPid; + + ///< Any user input is translated into desired RPM for each motor. + float frontLeftDesiredRpm; + float frontRightDesiredRpm; + float backLeftDesiredRpm; + float backRightDesiredRpm; + + // Ramp for each input + tap::algorithms::Ramp xInputRamp; + tap::algorithms::Ramp yInputRamp; + tap::algorithms::Ramp rInputRamp; + + // previous update time for ramp + float prevRampUpdate = 0.0f; + + // Ramp time + static constexpr float RAMP_TIME_MS = 500.0f; + + // Slope for ramp + static constexpr float RAMP_SLOPE = 1.0f / RAMP_TIME_MS; + + // Scale factor for converting joystick movement into RPM setpoint + static constexpr float rpmScaleFactor = 3500.0f; + + uint32_t prevDebugTime; + uint32_t prevPidUpdate; + + // Variables for managing UART messages sent to CV + uint32_t prevCVUpdate; + + // Conversions for CV Messages + const int16_t M_TO_MM = 1000; + const float DEG_TO_MILLIRAD = 17.453293; + +}; // class ChassisSpin2WinSubsystem + +} // namespace chassis + +} // namespace control + +#endif // CHASSIS_SUBSYSTEM_HPP_ \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index 20cf1fe..533ed27 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -128,33 +128,23 @@ void ChassisSubsystem::setTargetOutput(float x, float y, float r) { +x is forward, +y is right, +r is clockwise (turning right). Expressed in body frame. */ -// void ChassisSubsystem::setDesiredOutput(float x, float y, float r) -// { +void ChassisSubsystem::setDesiredOutput(float x, float y, float r) +{ -// x = tap::algorithms::limitVal(x,-1,1); -// y = tap::algorithms::limitVal(y,-1,1); -// r = tap::algorithms::limitVal(r,-1,1); + x = tap::algorithms::limitVal(x,-1,1); + y = tap::algorithms::limitVal(y,-1,1); + r = tap::algorithms::limitVal(r,-1,1); -// // x, y, and r contained between -1 and 1 -// // Normalize movement vector -// float norm = sqrt(x*x+y*y); -// if (norm > 1) { -// x = x / norm; -// y = y / norm; -// } - -// y = IS_Y_INVERTED ? -y : y; + // x, y, and r contained between -1 and 1 + // Normalize movement vector + float norm = sqrt(x*x+y*y); + if (norm > 1) { + x = x / norm; + y = y / norm; + } -// switch (wheelType) { -// case WheelType::omniwheel: -// setOmniwheelDesiredRPM(x, y, r); -// break; -// case WheelType::mecanum: -// default: -// setMecanumDesiredRPM(x, y, r); -// break; -// } -// } + y = IS_Y_INVERTED ? -y : y; +} /* Attempts to send IMU and wheel encoder data to CV over UART. diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index d010920..113e7b0 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -41,10 +41,9 @@ class ChassisSubsystem : public tap::control::Subsystem * Constructs a new ChassisSubsystem with default parameters specified in * the private section of this class. */ - ChassisSubsystem(src::Drivers *drivers, const SRobotConfig& robotConfig) + ChassisSubsystem(src::Drivers *drivers) : tap::control::Subsystem(drivers), drivers(drivers), - wheelType(robotConfig.wheelType), frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), @@ -59,7 +58,12 @@ class ChassisSubsystem : public tap::control::Subsystem // backRightDesiredRpm(0), prevCVUpdate(0) { - lqrController = std::make_unique(); + lqrController = std::make_unique( + CHASSIS_MASS_KG, + CHASSIS_YAW_INERTIA_KGM2, + MAX_CURRENT_OUTPUT, + rpmScaleFactor +); } ChassisSubsystem(const ChassisSubsystem &other) = delete; From b2fde1ccfbe1aa900f0e0e52e5dc19f427701037 Mon Sep 17 00:00:00 2001 From: antoinefischer Date: Thu, 29 Jan 2026 16:42:18 -0500 Subject: [PATCH 04/12] feat/testing turret logic (non-fonctional for now) --- .../src/control/Icra/icra_control.cpp | 2 +- .../subsystems/chassis/chassis_subsystem.cpp | 3 ++ .../turret/algorithms/turret_lqr.cpp | 54 ++++++++++--------- .../turret/algorithms/turret_lqr.hpp | 42 ++++++++------- .../subsystems/turret/turret_constants.hpp | 7 +-- .../subsystems/turret/turret_subsystem.cpp | 48 ++++++++++++----- .../subsystems/turret/turret_subsystem.hpp | 5 +- 7 files changed, 96 insertions(+), 65 deletions(-) diff --git a/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp b/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp index 518f130..672895c 100644 --- a/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp +++ b/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp @@ -110,7 +110,7 @@ void setDefaultStandardCommands(src::Drivers *) { theChassis.setDefaultCommand(&chassisDrive); theTurret.setDefaultCommand(&turretManualAim); - theFlywheel.setDefaultCommand(&flywheelStart); + //theFlywheel.setDefaultCommand(&flywheelStart); } /* add any starting commands to the scheduler here --------------------------*/ diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index 533ed27..b63e7d6 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -118,6 +118,9 @@ void ChassisSubsystem::updateRpmSetpoints() { } void ChassisSubsystem::setTargetOutput(float x, float y, float r) { +#ifdef IS_Y_INVERTED + if (IS_Y_INVERTED) y = -y; +#endif xInputRamp.setTarget(x); yInputRamp.setTarget(y); rInputRamp.setTarget(r); diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp index 1428d51..34fbd2c 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -1,40 +1,42 @@ #include "turret_lqr.hpp" -#include +#include namespace turret::algorithms { -TurretLqrController::TurretLqrController(float panInertia, float tiltInertia, - float Qscale, float Rscale) - : I_pan(panInertia), - I_tilt(tiltInertia) +TurretLqrController::TurretLqrController(float panInertia, + float tiltInertia, + float motorOutputMax, + float axisToMotorScale) + : I_pan_(panInertia), + I_tilt_(tiltInertia), + maxOut_(motorOutputMax), + scale_(axisToMotorScale) { - K_pan = computeLqrGain(I_pan, Qscale, Rscale); - K_tilt = computeLqrGain(I_tilt, Qscale, Rscale); + Kpan_ = defaultGains(I_pan_); + Ktilt_ = defaultGains(I_tilt_); } -// Called periodically to update the controller state -void TurretLqrController::update(float panAngle, float panRate, float panTarget, - float tiltAngle, float tiltRate, float tiltTarget) +void TurretLqrController::setGains(const Gains2& Kpan, const Gains2& Ktilt) { - float panError = panAngle - panTarget; - float tiltError = tiltAngle - tiltTarget; - - panVoltage = -(K_pan[0] * panError + K_pan[1] * panRate); // Power adjustment to add/remove for x axis - tiltVoltage = -(K_tilt[0] * tiltError + K_tilt[1] * tiltRate); // Power adjustment to add/remove for y axis + Kpan_ = Kpan; + Ktilt_ = Ktilt; } -float TurretLqrController::getPanVoltage() const { return panVoltage; } -float TurretLqrController::getTiltVoltage() const { return tiltVoltage; } - -// Model per axis: A=[0 1; 0 0], B=[0; 1/I], Q=diag(q,q), R=r -std::array TurretLqrController::computeLqrGain(float inertia, float Qscale, float Rscale) +std::array TurretLqrController::update(float panAngle, float panRate, float panRef, + float tiltAngle, float tiltRate, float tiltRef) { - float q = Qscale; - float r = Rscale; + const float uPan = -(Kpan_.k_pos * (panAngle - panRef) + Kpan_.k_vel * panRate); + const float uTilt = -(Ktilt_.k_pos * (tiltAngle - tiltRef) + Ktilt_.k_vel * tiltRate); - float k_angle = std::sqrt(q / r) * inertia; - float k_rate = std::sqrt(q * r); + float panCmd = uPan * scale_; + float tiltCmd = uTilt * scale_; - return { k_angle, k_rate }; + auto clamp = [this](float v){ return std::clamp(v, -maxOut_, +maxOut_); }; + return { clamp(panCmd), clamp(tiltCmd) }; +} + +TurretLqrController::Gains2 TurretLqrController::defaultGains(float dyn) +{ + return { 1.5f * dyn, 0.1f }; // test values, on va compute les vrais si les tests marchent } -} // namespace turret::algorithms +} // namespace turret::algorithms diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp index 1faa7ef..9c58e04 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp @@ -5,35 +5,37 @@ namespace turret::algorithms { -/* LQR controller for a 2-DOF turret (pan, tilt). + /* LQR controller for a 2-DOF turret (pan, tilt). Regulates angle/rate on each axis and outputs a motor command. */ class TurretLqrController { public: - TurretLqrController(float panInertia, float tiltInertia, - float Qscale = 1.0f, float Rscale = 1.0f); + struct Gains2 { float k_pos; float k_vel; }; - // Update the controller and compute outputs for pan/tilt (x,y axes). - void update(float panAngle, float panRate, float panTarget, - float tiltAngle, float tiltRate, float tiltTarget); + TurretLqrController(float panInertia, + float tiltInertia, + float motorOutputMax = 8000.0f, + float axisToMotorScale = 3500.0f); - float getPanVoltage() const; - float getTiltVoltage() const; + void setGains(const Gains2& Kpan, const Gains2& Ktilt); -private: - float I_pan; - float I_tilt; + // inputs are angles/rates (rad, rad/s) and refs (rad) + // returns motor commands in motor units + std::array update(float panAngle, float panRate, float panRef, + float tiltAngle, float tiltRate, float tiltRef); - // K = [k_angle, k_rate] for each axis (u = -K * [angle_error, rate]) - std::array K_pan; - std::array K_tilt; +private: + static Gains2 defaultGains(float dyn); - float panVoltage = 0.f; - float tiltVoltage = 0.f; +private: + float I_pan_; + float I_tilt_; + float maxOut_; + float scale_; - // Simple initializer for testing, will replace with CARE-based gains when computed - std::array computeLqrGain(float inertia, float Qscale, float Rscale); + Gains2 Kpan_{}; + Gains2 Ktilt_{}; }; -} // namespace turret::algorithms +} // namespace turret::algorithms -#endif // TURRET_LQR__HPP_ +#endif // TURRET_LQR__HPP_ \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp index 694f194..5f49b1f 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp @@ -46,8 +46,8 @@ static constexpr uint32_t TURRET_CV_UPDATE_PERIOD = 10; */ static constexpr float RPM_TO_DEGPERMS = 0.006; static constexpr float DEGREE_TO_MILLIRAD = 17.453293; -static constexpr float DEG_TO_RAD = DEGREE_TO_MILLIRAD / 1000.0f; -static constexpr float RPM_TO_RAD_S = RPM_TO_DEGPERMS * DEGREE_TO_MILLIRAD; +static constexpr float DEG_TO_RAD = 0.017453293f; +static constexpr float RPM_TO_RAD_S = 0.104719755f; /** @@ -63,4 +63,5 @@ static constexpr float HIGH_ROTATION = 0.95; static constexpr float TURRET_PAN_INERTIA = 0.0051f; // kg·m² static constexpr float TURRET_TILT_INERTIA = 0.0015f; // kg·m² static constexpr float TURRET_LQR_Q = 50.0f; -static constexpr float TURRET_LQR_R = 0.05f; \ No newline at end of file +static constexpr float TURRET_LQR_R = 0.05f; +static constexpr float TURRET_MASS_KG = 4.44628f; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index 34196da..406824c 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -59,22 +59,36 @@ void TurretSubsystem::refresh() { Run yaw controller and update motor output. */ void TurretSubsystem::runYawController(uint32_t dt) { - float error = yawDesiredPos - yawMotor->getEncoderWrapped(); - if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { - // If error is greater than 180deg then the shortest path to setpoint crosses zero - // So we add +/- 360deg to error to get the correct direction - // Avoids turret whipping around when position crosses zero - error = error - DjiMotor::ENC_RESOLUTION * getSign(error); - } + int64_t yawMeasUnwrapped = yawMotor->getEncoderUnwrapped(); + int64_t yawRefWrapped = static_cast(yawDesiredPos); + int64_t encRes = static_cast(DjiMotor::ENC_RESOLUTION); + + // Unwrap de la consigne: choisir la copie (k*encRes + wrapped) la plus proche de la mesure unwrapped pour pas qu'on saute randomly + int64_t base = (yawMeasUnwrapped / encRes) * encRes; + int64_t yawRefUnwrapped = base + yawRefWrapped; + + int64_t a = yawRefUnwrapped - encRes; + int64_t b = yawRefUnwrapped; + int64_t c = yawRefUnwrapped + encRes; + + auto dist = [&](int64_t v){ return llabs(v - yawMeasUnwrapped); }; + yawRefUnwrapped = (dist(a) < dist(b)) ? a : b; + yawRefUnwrapped = (dist(c) < dist(yawRefUnwrapped)) ? c : yawRefUnwrapped; + int64_t errorUnwrapped = yawRefUnwrapped - yawMeasUnwrapped; + int16_t currentRPM = yawMotor->getShaftRPM(); - float errDeg = DjiMotor::encoderToDegrees((int64_t)error); + float errDeg = DjiMotor::encoderToDegrees(errorUnwrapped); float errRad = errDeg * DEG_TO_RAD; + errRad = -errRad; float omega = static_cast(currentRPM) * RPM_TO_RAD_S; - lqrTurret.update(/*panAngle*/ errRad, /*panRate*/ omega, /*panTarget*/ 0.0f, - /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltTarget*/ 0.0f); - yawMotor->setDesiredOutput(lqrTurret.getPanVoltage()); + auto cmd = lqrTurret.update( + /*panAngle*/ errRad, /*panRate*/ omega, /*panRef*/ 0.0f, + /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltRef*/ 0.0f + ); + + yawMotor->setDesiredOutput(cmd[0]); // cascadedYawController.update(error, currentRPM, dt); @@ -90,11 +104,17 @@ void TurretSubsystem::runPitchController(uint32_t dt) { float errDeg = pitchMotor.encoderToDegrees((int64_t)error); float errRad = errDeg * DEG_TO_RAD; + errRad = -errRad; + float omega = static_cast(currentRPM) * RPM_TO_RAD_S; - lqrTurret.update(/*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panTarget*/ 0.0f, - /*tiltAngle*/ errRad, /*tiltRate*/ omega, /*tiltTarget*/ 0.0f); - pitchMotor.setDesiredOutput(lqrTurret.getTiltVoltage()); + auto cmd = lqrTurret.update( + /*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panRef*/ 0.0f, + /*tiltAngle*/ errRad, /*tiltRate*/ omega, /*tiltRef*/ 0.0f + ); + + pitchMotor.setDesiredOutput(cmd[1]); + // cascadedPitchController.update(error, currentRPM, dt); // pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp index 9dde172..fddef06 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp @@ -35,7 +35,10 @@ class TurretSubsystem : public tap::control::Subsystem pitchMotor(drivers, PITCH_MOTOR_ID, CAN_BUS_MOTORS, PITCH_IS_INVERTED, "pitch motor"), // cascadedPitchController(PITCH_OUTER_PID_CONFIG, PITCH_INNER_PID_CONFIG), // cascadedYawController(YAW_OUTER_PID_CONFIG, YAW_INNER_PID_CONFIG), - lqrTurret(TURRET_PAN_INERTIA, TURRET_TILT_INERTIA, /*Q*/TURRET_LQR_Q, /*R*/TURRET_LQR_R), + lqrTurret(TURRET_PAN_INERTIA, + TURRET_TILT_INERTIA, + /*motorOutputMax*/ 8000.0f, + /*axisToMotorScale*/ 3500.0f), yawDesiredPos(YAW_NEUTRAL_POS), pitchDesiredPos(PITCH_NEUTRAL_POS) { From 3251f68d800d4555a9aba9265ba7f3ed1a015b76 Mon Sep 17 00:00:00 2001 From: antoinefischer Date: Fri, 13 Feb 2026 20:03:28 -0500 Subject: [PATCH 05/12] Turret LQR functional, but needs deadzone and overcorrects. --- .../turret/algorithms/turret_lqr.cpp | 19 ++- .../turret/algorithms/turret_lqr.hpp | 2 +- .../subsystems/turret/turret_constants.hpp | 4 +- .../subsystems/turret/turret_subsystem.cpp | 159 ++++++++++++------ .../subsystems/turret/turret_subsystem.hpp | 5 +- 5 files changed, 130 insertions(+), 59 deletions(-) diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp index 34fbd2c..f60d555 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -12,8 +12,11 @@ TurretLqrController::TurretLqrController(float panInertia, maxOut_(motorOutputMax), scale_(axisToMotorScale) { - Kpan_ = defaultGains(I_pan_); - Ktilt_ = defaultGains(I_tilt_); + // Gains LQR optimaux calcules avec Simulink CARE solver + // Kpan_ = {10.00f, 1.72f}; + Ktilt_ = {20.00f, 2.68f}; + Kpan_ = {31.62f, 10.15f}; + // Ktilt_ = {31.62f, 10.13f}; } void TurretLqrController::setGains(const Gains2& Kpan, const Gains2& Ktilt) @@ -25,18 +28,18 @@ void TurretLqrController::setGains(const Gains2& Kpan, const Gains2& Ktilt) std::array TurretLqrController::update(float panAngle, float panRate, float panRef, float tiltAngle, float tiltRate, float tiltRef) { - const float uPan = -(Kpan_.k_pos * (panAngle - panRef) + Kpan_.k_vel * panRate); - const float uTilt = -(Ktilt_.k_pos * (tiltAngle - tiltRef) + Ktilt_.k_vel * tiltRate); + const float errPan = panAngle - panRef; + const float errTilt = tiltAngle - tiltRef; - float panCmd = uPan * scale_; - float tiltCmd = uTilt * scale_; + const float uPan = -(Kpan_.k_pos * errPan + Kpan_.k_vel * panRate) * scale_; + const float uTilt = -(Ktilt_.k_pos * errTilt + Ktilt_.k_vel * tiltRate) * scale_; auto clamp = [this](float v){ return std::clamp(v, -maxOut_, +maxOut_); }; - return { clamp(panCmd), clamp(tiltCmd) }; + return { clamp(uPan), clamp(uTilt) }; } TurretLqrController::Gains2 TurretLqrController::defaultGains(float dyn) { - return { 1.5f * dyn, 0.1f }; // test values, on va compute les vrais si les tests marchent + return {10.00f, 1.72f}; } } // namespace turret::algorithms diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp index 9c58e04..239093e 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp @@ -15,7 +15,7 @@ class TurretLqrController TurretLqrController(float panInertia, float tiltInertia, float motorOutputMax = 8000.0f, - float axisToMotorScale = 3500.0f); + float axisToMotorScale = 650.0f); void setGains(const Gains2& Kpan, const Gains2& Ktilt); diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp index 5f49b1f..643a286 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp @@ -60,8 +60,8 @@ static constexpr float HIGH_ROTATION = 0.95; /** * LQR parameters (need to be computed with Simulink or similar CARE solver if you want to change them) */ -static constexpr float TURRET_PAN_INERTIA = 0.0051f; // kg·m² -static constexpr float TURRET_TILT_INERTIA = 0.0015f; // kg·m² +static constexpr float TURRET_PAN_INERTIA = 0.048f; // kg·m² +static constexpr float TURRET_TILT_INERTIA = 0.041f; // kg·m² static constexpr float TURRET_LQR_Q = 50.0f; static constexpr float TURRET_LQR_R = 0.05f; static constexpr float TURRET_MASS_KG = 4.44628f; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index 406824c..7bb2adb 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -58,66 +58,131 @@ void TurretSubsystem::refresh() { /* Run yaw controller and update motor output. */ -void TurretSubsystem::runYawController(uint32_t dt) { - int64_t yawMeasUnwrapped = yawMotor->getEncoderUnwrapped(); - int64_t yawRefWrapped = static_cast(yawDesiredPos); - int64_t encRes = static_cast(DjiMotor::ENC_RESOLUTION); - - // Unwrap de la consigne: choisir la copie (k*encRes + wrapped) la plus proche de la mesure unwrapped pour pas qu'on saute randomly - int64_t base = (yawMeasUnwrapped / encRes) * encRes; - int64_t yawRefUnwrapped = base + yawRefWrapped; - - int64_t a = yawRefUnwrapped - encRes; - int64_t b = yawRefUnwrapped; - int64_t c = yawRefUnwrapped + encRes; - - auto dist = [&](int64_t v){ return llabs(v - yawMeasUnwrapped); }; - yawRefUnwrapped = (dist(a) < dist(b)) ? a : b; - yawRefUnwrapped = (dist(c) < dist(yawRefUnwrapped)) ? c : yawRefUnwrapped; - int64_t errorUnwrapped = yawRefUnwrapped - yawMeasUnwrapped; +// void TurretSubsystem::runYawController(uint32_t dt) { + +// //Sanity check (testing if the motor is online) Uncomment this block if you want to check that turret control at all works (turret will slowly spin) +// // yawMotor->setDesiredOutput(1500); +// // return; + +// int64_t yawMeasUnwrapped = yawMotor->getEncoderUnwrapped(); +// int64_t yawRefWrapped = static_cast(yawDesiredPos); +// int64_t encRes = static_cast(DjiMotor::ENC_RESOLUTION); + +// // Unwrap de la consigne: choisir la copie (k*encRes + wrapped) la plus proche de la mesure unwrapped pour pas qu'on saute randomly +// int64_t base = (yawMeasUnwrapped / encRes) * encRes; +// int64_t yawRefUnwrapped = base + yawRefWrapped; + +// int64_t a = yawRefUnwrapped - encRes; +// int64_t b = yawRefUnwrapped; +// int64_t c = yawRefUnwrapped + encRes; + +// auto dist = [&](int64_t v){ return llabs(v - yawMeasUnwrapped); }; +// yawRefUnwrapped = (dist(a) < dist(b)) ? a : b; +// yawRefUnwrapped = (dist(c) < dist(yawRefUnwrapped)) ? c : yawRefUnwrapped; +// int64_t errorUnwrapped = yawRefUnwrapped - yawMeasUnwrapped; - int16_t currentRPM = yawMotor->getShaftRPM(); +// int16_t currentRPM = yawMotor->getShaftRPM(); - float errDeg = DjiMotor::encoderToDegrees(errorUnwrapped); - float errRad = errDeg * DEG_TO_RAD; - errRad = -errRad; - float omega = static_cast(currentRPM) * RPM_TO_RAD_S; +// float errDeg = DjiMotor::encoderToDegrees(errorUnwrapped); +// float errRad = errDeg * DEG_TO_RAD; +// float omega = static_cast(currentRPM) * RPM_TO_RAD_S; +// float errRate = -omega; - auto cmd = lqrTurret.update( - /*panAngle*/ errRad, /*panRate*/ omega, /*panRef*/ 0.0f, - /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltRef*/ 0.0f - ); +// auto cmd = lqrTurret.update( +// /*panAngle*/ errRad, /*panRate*/ errRate, /*panRef*/ 0.0f, +// /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltRef*/ 0.0f +// ); - yawMotor->setDesiredOutput(cmd[0]); +// const float ERR_EPS = 0.5f * DEG_TO_RAD; // ~0.5 deg +// const float OMEGA_EPS = 2.0f * DEG_TO_RAD; // ~2 deg/s - // cascadedYawController.update(error, currentRPM, dt); +// if (std::fabs(errRad) < ERR_EPS && std::fabs(omega) < OMEGA_EPS) { +// yawMotor->setDesiredOutput(0); +// return; +// } +// float u = cmd[0]; +// u = std::clamp(u, -8000.0f, 8000.0f); +// yawMotor->setDesiredOutput(u); - // yawMotor->setDesiredOutput(cascadedYawController.getOutput()); -} -/* - Run pitch controller and update motor output. -*/ -void TurretSubsystem::runPitchController(uint32_t dt) { - float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); - int16_t currentRPM = pitchMotor.getShaftRPM(); +// // cascadedYawController.update(error, currentRPM, dt); - float errDeg = pitchMotor.encoderToDegrees((int64_t)error); - float errRad = errDeg * DEG_TO_RAD; - errRad = -errRad; +// // yawMotor->setDesiredOutput(cascadedYawController.getOutput()); +// } + +// /* +// Run pitch controller and update motor output. +// */ +// void TurretSubsystem::runPitchController(uint32_t dt) { +// float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); +// int16_t currentRPM = pitchMotor.getShaftRPM(); + +// float errDeg = pitchMotor.encoderToDegrees((int64_t)error); +// float errRad = errDeg * DEG_TO_RAD; +// float omega = static_cast(currentRPM) * RPM_TO_RAD_S; +// float errRate = -omega; + +// auto cmd = lqrTurret.update( +// /*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panRef*/ 0.0f, +// /*tiltAngle*/ errRad, /*tiltRate*/ errRate, /*tiltRef*/ 0.0f +// ); - float omega = static_cast(currentRPM) * RPM_TO_RAD_S; +// const float ERR_EPS = 0.5f * DEG_TO_RAD; // ~0.5 deg +// const float OMEGA_EPS = 2.0f * DEG_TO_RAD; // ~2 deg/s - auto cmd = lqrTurret.update( - /*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panRef*/ 0.0f, - /*tiltAngle*/ errRad, /*tiltRate*/ omega, /*tiltRef*/ 0.0f - ); +// if (std::fabs(errRad) < ERR_EPS && std::fabs(omega) < OMEGA_EPS) { +// pitchMotor.setDesiredOutput(0); +// return; +// } +// float u = cmd[1]; +// u = std::clamp(u, -8000.0f, 8000.0f); +// pitchMotor.setDesiredOutput(u); - pitchMotor.setDesiredOutput(cmd[1]); +// // cascadedPitchController.update(error, currentRPM, dt); - // cascadedPitchController.update(error, currentRPM, dt); +// // pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); +// } - // pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); +void TurretSubsystem::runYawController(uint32_t dt) { + int32_t error = static_cast(yawDesiredPos) + - static_cast(yawMotor->getEncoderWrapped()); + + if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { + error = error - DjiMotor::ENC_RESOLUTION * getSign(error); + } + + int16_t currentRPM = yawMotor->getShaftRPM(); + + float errDeg = yawMotor->encoderToDegrees((int64_t)error); + float errRad = errDeg * DEG_TO_RAD; + float omega = static_cast(currentRPM) * RPM_TO_RAD_S; + + if (std::fabs(errRad) < 0.5f * DEG_TO_RAD && std::fabs(omega) < 0.2f) { + yawMotor->setDesiredOutput(0); + return; + } + + auto cmd = lqrTurret.update(errRad, omega, 0.0f, 0.0f, 0.0f, 0.0f); + yawMotor->setDesiredOutput(cmd[0]); +} + +void TurretSubsystem::runPitchController(uint32_t dt) { + int32_t error = static_cast(pitchDesiredPos) + - static_cast(pitchMotor.getEncoderWrapped()); + + int16_t currentRPM = pitchMotor.getShaftRPM(); + + float errDeg = pitchMotor.encoderToDegrees(static_cast(error)); + float errRad = errDeg * DEG_TO_RAD; + float omega = static_cast(currentRPM) * RPM_TO_RAD_S; + + if (std::fabs(errRad) < 0.5f * DEG_TO_RAD && std::fabs(omega) < 0.2f) { + pitchMotor.setDesiredOutput(0); + return; + } + + auto cmd = lqrTurret.update(0.0f, 0.0f, 0.0f, errRad, omega, 0.0f); + pitchMotor.setDesiredOutput(-cmd[1]); } /* diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp index fddef06..0793e76 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp @@ -38,7 +38,7 @@ class TurretSubsystem : public tap::control::Subsystem lqrTurret(TURRET_PAN_INERTIA, TURRET_TILT_INERTIA, /*motorOutputMax*/ 8000.0f, - /*axisToMotorScale*/ 3500.0f), + /*axisToMotorScale*/ 350.0f), yawDesiredPos(YAW_NEUTRAL_POS), pitchDesiredPos(PITCH_NEUTRAL_POS) { @@ -103,6 +103,9 @@ class TurretSubsystem : public tap::control::Subsystem float yawDesiredPos; float pitchDesiredPos; + bool yawInDeadzone_ = false; + bool pitchInDeadzone_ = false; + // Time variables for fixed rate tasks uint32_t prevDebugUpdate; uint32_t prevControllerUpdate; From 8f561156d53cb995b2436398022bdc8491614bc3 Mon Sep 17 00:00:00 2001 From: Glory Date: Fri, 13 Feb 2026 20:51:10 -0500 Subject: [PATCH 06/12] fixed yawn direction --- .gitignore | 2 + .../subsystems/turret/turret_subsystem.cpp | 89 +------------------ 2 files changed, 4 insertions(+), 87 deletions(-) diff --git a/.gitignore b/.gitignore index db4726b..3d71950 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,8 @@ PolySTAR-Taproot-project/build PolySTAR-Taproot-project/Pipfile.lock +PolySTAR-Taproot-project/openocd.cfg + project.xml.log .vscode/.cortex-debug.peripherals.state.json diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index 7bb2adb..e59055d 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -58,94 +58,9 @@ void TurretSubsystem::refresh() { /* Run yaw controller and update motor output. */ -// void TurretSubsystem::runYawController(uint32_t dt) { - -// //Sanity check (testing if the motor is online) Uncomment this block if you want to check that turret control at all works (turret will slowly spin) -// // yawMotor->setDesiredOutput(1500); -// // return; - -// int64_t yawMeasUnwrapped = yawMotor->getEncoderUnwrapped(); -// int64_t yawRefWrapped = static_cast(yawDesiredPos); -// int64_t encRes = static_cast(DjiMotor::ENC_RESOLUTION); - -// // Unwrap de la consigne: choisir la copie (k*encRes + wrapped) la plus proche de la mesure unwrapped pour pas qu'on saute randomly -// int64_t base = (yawMeasUnwrapped / encRes) * encRes; -// int64_t yawRefUnwrapped = base + yawRefWrapped; - -// int64_t a = yawRefUnwrapped - encRes; -// int64_t b = yawRefUnwrapped; -// int64_t c = yawRefUnwrapped + encRes; - -// auto dist = [&](int64_t v){ return llabs(v - yawMeasUnwrapped); }; -// yawRefUnwrapped = (dist(a) < dist(b)) ? a : b; -// yawRefUnwrapped = (dist(c) < dist(yawRefUnwrapped)) ? c : yawRefUnwrapped; -// int64_t errorUnwrapped = yawRefUnwrapped - yawMeasUnwrapped; - -// int16_t currentRPM = yawMotor->getShaftRPM(); - -// float errDeg = DjiMotor::encoderToDegrees(errorUnwrapped); -// float errRad = errDeg * DEG_TO_RAD; -// float omega = static_cast(currentRPM) * RPM_TO_RAD_S; -// float errRate = -omega; - -// auto cmd = lqrTurret.update( -// /*panAngle*/ errRad, /*panRate*/ errRate, /*panRef*/ 0.0f, -// /*tiltAngle*/ 0.0f, /*tiltRate*/ 0.0f, /*tiltRef*/ 0.0f -// ); - -// const float ERR_EPS = 0.5f * DEG_TO_RAD; // ~0.5 deg -// const float OMEGA_EPS = 2.0f * DEG_TO_RAD; // ~2 deg/s - -// if (std::fabs(errRad) < ERR_EPS && std::fabs(omega) < OMEGA_EPS) { -// yawMotor->setDesiredOutput(0); -// return; -// } -// float u = cmd[0]; -// u = std::clamp(u, -8000.0f, 8000.0f); -// yawMotor->setDesiredOutput(u); - - -// // cascadedYawController.update(error, currentRPM, dt); - -// // yawMotor->setDesiredOutput(cascadedYawController.getOutput()); -// } - -// /* -// Run pitch controller and update motor output. -// */ -// void TurretSubsystem::runPitchController(uint32_t dt) { -// float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); -// int16_t currentRPM = pitchMotor.getShaftRPM(); - -// float errDeg = pitchMotor.encoderToDegrees((int64_t)error); -// float errRad = errDeg * DEG_TO_RAD; -// float omega = static_cast(currentRPM) * RPM_TO_RAD_S; -// float errRate = -omega; - -// auto cmd = lqrTurret.update( -// /*panAngle*/ 0.0f, /*panRate*/ 0.0f, /*panRef*/ 0.0f, -// /*tiltAngle*/ errRad, /*tiltRate*/ errRate, /*tiltRef*/ 0.0f -// ); - -// const float ERR_EPS = 0.5f * DEG_TO_RAD; // ~0.5 deg -// const float OMEGA_EPS = 2.0f * DEG_TO_RAD; // ~2 deg/s - -// if (std::fabs(errRad) < ERR_EPS && std::fabs(omega) < OMEGA_EPS) { -// pitchMotor.setDesiredOutput(0); -// return; -// } -// float u = cmd[1]; -// u = std::clamp(u, -8000.0f, 8000.0f); -// pitchMotor.setDesiredOutput(u); - -// // cascadedPitchController.update(error, currentRPM, dt); - -// // pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); -// } - void TurretSubsystem::runYawController(uint32_t dt) { - int32_t error = static_cast(yawDesiredPos) - - static_cast(yawMotor->getEncoderWrapped()); + int32_t error = static_cast(yawMotor->getEncoderWrapped()) + - static_cast(yawDesiredPos) ; if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { error = error - DjiMotor::ENC_RESOLUTION * getSign(error); From 0e8d6fba7bc36e72b1dad2f270634c8f4ead8a68 Mon Sep 17 00:00:00 2001 From: Glory Date: Sat, 14 Feb 2026 16:10:47 -0500 Subject: [PATCH 07/12] right values of tilt and pan --- .../src/subsystems/turret/algorithms/turret_lqr.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp index f60d555..c4e00fc 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -14,8 +14,8 @@ TurretLqrController::TurretLqrController(float panInertia, { // Gains LQR optimaux calcules avec Simulink CARE solver // Kpan_ = {10.00f, 1.72f}; - Ktilt_ = {20.00f, 2.68f}; - Kpan_ = {31.62f, 10.15f}; + Ktilt_ = {31.62f, 1.61f}; + Kpan_ = {31.62f, 3.62f}; // Ktilt_ = {31.62f, 10.13f}; } From a7c0908be0cdbdd303e8ac11687d5d0ad15edd63 Mon Sep 17 00:00:00 2001 From: Glory Date: Sat, 14 Feb 2026 17:07:53 -0500 Subject: [PATCH 08/12] change chassis simulated LQR to true LQR --- .../chassis/algorithms/chassis_lqr.cpp | 10 +++-- .../chassis/algorithms/chassis_lqr.hpp | 45 ++++++++++--------- .../subsystems/chassis/chassis_subsystem.cpp | 17 +++++-- .../subsystems/chassis/chassis_subsystem.hpp | 4 +- 4 files changed, 47 insertions(+), 29 deletions(-) diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp index 8ebb6f7..ed4abfa 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -16,9 +16,13 @@ ChassisLqrController::ChassisLqrController(float mass, scale_(axisToMotorScale) { // Default conservative gains. Replace these values via setGains() whenever LQR K values need to be recomputed if big changes have been made to robot. - Kx_ = defaultGains(m_); - Ky_ = defaultGains(m_); - Kt_ = defaultGains(I_); + // Kx_ = defaultGains(m_); + // Ky_ = defaultGains(m_); + // Kt_ = defaultGains(I_); + + Kx_ = {63.24f, 11.25f}; + Ky_ = {63.24f, 11.25f}; + Kt_ = {15.81f, 2.45f}; } void ChassisLqrController::setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt) diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp index 97ed8aa..2492db4 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -14,27 +14,30 @@ namespace chassis::algorithms class ChassisLqrController { public: - struct Gains2 { float k_pos; float k_vel; }; - - /* mass: chassis mass (kg) - inertia: chassis yaw inertia about CoM (kg·m²) - motorOutputMax: clamp for motor command (C620 DJI currently) - axisToMotorScale: maps axis effort to per-wheel command (rpmScaleFactor) */ - ChassisLqrController(float mass, - float inertia, - float motorOutputMax = 8000.0f, - float axisToMotorScale = 3500.0f); - - // Optional: inject gains computed offline (CARE). - void setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt); - - /* Update controller. - Inputs are CURRENT body velocities and references, normalized to [-1,1] (state-based control). - Derivatives dvx/dvy/dw can be 0 if not available. - Returns motor commands: {FL, FR, BL, BR} clamped to motorOutputMax. */ - std::array update(float vx, float dvx, float vx_ref, - float vy, float dvy, float vy_ref, - float w, float dw, float w_ref); + struct Gains2 { float k_pos; float k_vel; }; + + /* mass: chassis mass (kg) + inertia: chassis yaw inertia about CoM (kg·m²) + motorOutputMax: clamp for motor command (C620 DJI currently) + axisToMotorScale: maps axis effort to per-wheel command (rpmScaleFactor) */ + ChassisLqrController(float mass, + float inertia, + float motorOutputMax = 8000.0f, + float axisToMotorScale = 3500.0f); + + // Optional: inject gains computed offline (CARE). + void setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt); + + /* Update controller. + Inputs are CURRENT body velocities and references, normalized to [-1,1] (state-based control). + Derivatives dvx/dvy/dw can be 0 if not available. + Returns motor commands: {FL, FR, BL, BR} clamped to motorOutputMax. */ + std::array update(float vx, float dvx, float vx_ref, + float vy, float dvy, float vy_ref, + float w, float dw, float w_ref); + // float prevVx; + // float prevVy; + // float prevW; private: // Simple default gains, will replace once robot has been simulated to get CARE-computed gains. diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index b63e7d6..aeec67b 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -1,3 +1,5 @@ + + #include "chassis_subsystem.hpp" #include "tap/communication/serial/remote.hpp" @@ -45,11 +47,18 @@ void ChassisSubsystem::refresh() { float vx=0.f, vy=0.f, w=0.f; rpmToBody(fl, fr, bl, br, vx, vy, w); - // If we don't have estimated derivatives, we pass 0.f for dvx/dvy/dw + float dvx = (vx - prevVx) / dt; + float dvy = (vy - prevVy) / dt; + float dw = (w - prevW ) / dt; + + prevVx = vx; + prevVy = vy; + prevW = w; + auto cmd = lqrController->update( - vx, 0.f, vxRef, - vy, 0.f, vyRef, - w, 0.f, wRef + vx, dvx, vxRef, + vy, dvy, vyRef, + w, dw, wRef ); frontLeftMotor.setDesiredOutput(cmd[0]); diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index 113e7b0..1967823 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -36,7 +36,9 @@ class ChassisSubsystem : public tap::control::Subsystem * than enough for what you are doing. */ static constexpr float MAX_CURRENT_OUTPUT = 8000.0f; - + float prevVx; + float prevVy; + float prevW; /** * Constructs a new ChassisSubsystem with default parameters specified in * the private section of this class. From 99d6db7f2d3dfbce91858b91a5ce9da2f0211481 Mon Sep 17 00:00:00 2001 From: Glory Date: Fri, 27 Feb 2026 20:58:12 -0500 Subject: [PATCH 09/12] chore: added comments, removed old code commented --- .../subsystems/chassis/algorithms/chassis_lqr.cpp | 2 +- .../subsystems/chassis/algorithms/chassis_lqr.hpp | 5 ----- .../src/subsystems/chassis/chassis_subsystem.cpp | 5 +++++ .../src/subsystems/chassis/chassis_subsystem.hpp | 2 +- .../src/subsystems/turret/algorithms/turret_lqr.cpp | 2 -- .../src/subsystems/turret/turret_subsystem.cpp | 12 ++++++++++++ .../src/subsystems/turret/turret_subsystem.hpp | 3 ++- 7 files changed, 21 insertions(+), 10 deletions(-) diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp index ed4abfa..9c0059c 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -15,7 +15,7 @@ ChassisLqrController::ChassisLqrController(float mass, maxOut_(motorOutputMax), scale_(axisToMotorScale) { - // Default conservative gains. Replace these values via setGains() whenever LQR K values need to be recomputed if big changes have been made to robot. + // Replace these values via setGains() whenever LQR K values need to be recomputed if big changes have been made to robot. // Kx_ = defaultGains(m_); // Ky_ = defaultGains(m_); // Kt_ = defaultGains(I_); diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp index 2492db4..ac420bc 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -25,7 +25,6 @@ class ChassisLqrController float motorOutputMax = 8000.0f, float axisToMotorScale = 3500.0f); - // Optional: inject gains computed offline (CARE). void setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt); /* Update controller. @@ -35,12 +34,8 @@ class ChassisLqrController std::array update(float vx, float dvx, float vx_ref, float vy, float dvy, float vy_ref, float w, float dw, float w_ref); - // float prevVx; - // float prevVy; - // float prevW; private: - // Simple default gains, will replace once robot has been simulated to get CARE-computed gains. static Gains2 defaultGains(float dyn); private: diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp index aeec67b..be5058a 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.cpp @@ -47,14 +47,19 @@ void ChassisSubsystem::refresh() { float vx=0.f, vy=0.f, w=0.f; rpmToBody(fl, fr, bl, br, vx, vy, w); + // Calculate the robot's current acceleration (change in speed divided by time). float dvx = (vx - prevVx) / dt; float dvy = (vy - prevVy) / dt; float dw = (w - prevW ) / dt; + // Save the current speeds to use as the "previous" speeds during the next loop. prevVx = vx; prevVy = vy; prevW = w; + + // our current speed, current acceleration, and target speed (Ref) + // It spits out the optimal electrical commands (cmd) for all 4 motors. auto cmd = lqrController->update( vx, dvx, vxRef, vy, dvy, vyRef, diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index 1967823..7e8de30 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -142,7 +142,7 @@ class ChassisSubsystem : public tap::control::Subsystem float vyRef = 0.0f; // desired rightward speed in [-1, 1] float wRef = 0.0f; // desired CW rotation in [-1, 1] - //] Previous time the LQR control loop was updated + // Previous time the LQR control loop was updated uint32_t prevControlUpdate = 0; // ///< Any user input is translated into desired RPM for each motor. diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp index c4e00fc..74966a8 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -13,10 +13,8 @@ TurretLqrController::TurretLqrController(float panInertia, scale_(axisToMotorScale) { // Gains LQR optimaux calcules avec Simulink CARE solver - // Kpan_ = {10.00f, 1.72f}; Ktilt_ = {31.62f, 1.61f}; Kpan_ = {31.62f, 3.62f}; - // Ktilt_ = {31.62f, 10.13f}; } void TurretLqrController::setGains(const Gains2& Kpan, const Gains2& Ktilt) diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index e59055d..7343327 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -59,43 +59,55 @@ void TurretSubsystem::refresh() { Run yaw controller and update motor output. */ void TurretSubsystem::runYawController(uint32_t dt) { + // Calculate the distance between our current angle and the target angle int32_t error = static_cast(yawMotor->getEncoderWrapped()) - static_cast(yawDesiredPos) ; + + // Make sure the turret takes the shortest path instead of spinning the long way around (wrapped) if (abs(error) >= DjiMotor::ENC_RESOLUTION/2) { error = error - DjiMotor::ENC_RESOLUTION * getSign(error); } + // Check how fast the turret is currently spinning. int16_t currentRPM = yawMotor->getShaftRPM(); + // Convert the raw motor hardware numbers into standard math units (radians) float errDeg = yawMotor->encoderToDegrees((int64_t)error); float errRad = errDeg * DEG_TO_RAD; float omega = static_cast(currentRPM) * RPM_TO_RAD_S; + // If we are super close to the target and barely moving, turn the motor off so it doesn't jitter if (std::fabs(errRad) < 0.5f * DEG_TO_RAD && std::fabs(omega) < 0.2f) { yawMotor->setDesiredOutput(0); return; } + // Calculate exactly how much power is needed, then send that power to the motor. auto cmd = lqrTurret.update(errRad, omega, 0.0f, 0.0f, 0.0f, 0.0f); yawMotor->setDesiredOutput(cmd[0]); } void TurretSubsystem::runPitchController(uint32_t dt) { + // Calculate how far off we are from where we want to point. int32_t error = static_cast(pitchDesiredPos) - static_cast(pitchMotor.getEncoderWrapped()); + // Check how fast the turret is currently tilting. int16_t currentRPM = pitchMotor.getShaftRPM(); + // Convert the raw motor hardware numbers into standard math units (radians). float errDeg = pitchMotor.encoderToDegrees(static_cast(error)); float errRad = errDeg * DEG_TO_RAD; float omega = static_cast(currentRPM) * RPM_TO_RAD_S; + // If we are super close to the target and barely moving, turn the motor off so it doesn't jitter if (std::fabs(errRad) < 0.5f * DEG_TO_RAD && std::fabs(omega) < 0.2f) { pitchMotor.setDesiredOutput(0); return; } + // Calculate the needed power and apply it to the motor (reversed with a '-' to match the physical wiring). auto cmd = lqrTurret.update(0.0f, 0.0f, 0.0f, errRad, omega, 0.0f); pitchMotor.setDesiredOutput(-cmd[1]); } diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp index 0793e76..f39e989 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.hpp @@ -45,7 +45,6 @@ class TurretSubsystem : public tap::control::Subsystem } TurretSubsystem(const TurretSubsystem &other) = delete; - TurretSubsystem &operator=(const TurretSubsystem &other) = delete; ~TurretSubsystem() = default; @@ -97,6 +96,8 @@ class TurretSubsystem : public tap::control::Subsystem // // Motor Controllers for position control // CascadedPid cascadedPitchController; // CascadedPid cascadedYawController; + + // Linear Quadratic Regulator for optimal turret stability. TurretLqrController lqrTurret; // Position setpoints for turret, in encoder ticks From 53b5f2d7460adb573feda9cb78f738c0cee13f22 Mon Sep 17 00:00:00 2001 From: TsarraG Date: Fri, 20 Mar 2026 20:47:01 -0400 Subject: [PATCH 10/12] test: lqr polymorphism --- .../chassis/algorithms/chassis_gains.hpp | 9 ++++++++ .../chassis/algorithms/chassis_lqr.cpp | 17 +++++++------- .../chassis/algorithms/chassis_lqr.hpp | 5 +---- .../subsystems/chassis/chassis_constants.hpp | 22 +++++++++++++++++++ 4 files changed, 41 insertions(+), 12 deletions(-) create mode 100644 PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp new file mode 100644 index 0000000..8963731 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp @@ -0,0 +1,9 @@ +#pragma once + +namespace control::chassis::algorithms +{ + struct Gains2 { + float k_pos; + float k_vel; + }; +} \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp index 9c0059c..9092d08 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -1,4 +1,5 @@ #include "chassis_lqr.hpp" +#include "chassis_constants.hpp" #include // std::clamp namespace control @@ -20,16 +21,16 @@ ChassisLqrController::ChassisLqrController(float mass, // Ky_ = defaultGains(m_); // Kt_ = defaultGains(I_); - Kx_ = {63.24f, 11.25f}; - Ky_ = {63.24f, 11.25f}; - Kt_ = {15.81f, 2.45f}; + Kx_ = Kx; + Ky_ = Ky; + Kt_ = Kt; } -void ChassisLqrController::setGains(const Gains2& Kx, const Gains2& Ky, const Gains2& Kt) +void ChassisLqrController::setGains(const Gains2& kx, const Gains2& ky, const Gains2& kt) { - Kx_ = Kx; - Ky_ = Ky; - Kt_ = Kt; + Kx_ = kx; + Ky_ = ky; + Kt_ = kt; } std::array ChassisLqrController::update(float vx, float dvx, float vx_ref, @@ -51,7 +52,7 @@ std::array ChassisLqrController::update(float vx, float dvx, float vx_r return { clamp(fl), clamp(fr), clamp(bl), clamp(br) }; } -ChassisLqrController::Gains2 ChassisLqrController::defaultGains(float dyn) +Gains2 ChassisLqrController::defaultGains(float dyn) { return { 1.5f * dyn, 0.1f }; } diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp index ac420bc..dd1bfcb 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -1,8 +1,7 @@ #ifndef CHASSIS_LQR_HPP_ #define CHASSIS_LQR_HPP_ - #include - +#include "chassis_gains.hpp" namespace control { namespace chassis::algorithms @@ -14,8 +13,6 @@ namespace chassis::algorithms class ChassisLqrController { public: - struct Gains2 { float k_pos; float k_vel; }; - /* mass: chassis mass (kg) inertia: chassis yaw inertia about CoM (kg·m²) motorOutputMax: clamp for motor command (C620 DJI currently) diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp index 60ca53a..9e3e979 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp @@ -1,4 +1,6 @@ + #pragma once +#include "./algorithms/chassis_gains.hpp" /** * Chassis wheel velocity PID: A PD controller for chassis wheel RPM. The PID parameters for the * controller are listed below. @@ -81,20 +83,40 @@ static constexpr uint32_t CHASSIS_DEBUG_MESSAGE_DELAY_MS = 100; */ #ifdef TARGET_HERO static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_SENTRY static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_STANDARD static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_ICRA static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_SPIN_TO_WIN static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif From 79bd0aecfcac8ebe9b7436d37b61d628f3b52662 Mon Sep 17 00:00:00 2001 From: Glory Date: Fri, 20 Mar 2026 21:06:51 -0400 Subject: [PATCH 11/12] feat: polymorphism implemented and tested --- .gitignore | 3 +- PolySTAR-Taproot-project/openocd.cfg | 12 +----- .../chassis/algorithms/chassis_gains.hpp | 37 ++++++++++++++++++- .../chassis/algorithms/chassis_lqr.cpp | 5 +-- .../chassis/algorithms/chassis_lqr.hpp | 2 +- .../subsystems/chassis/chassis_constants.hpp | 22 ----------- 6 files changed, 43 insertions(+), 38 deletions(-) diff --git a/.gitignore b/.gitignore index 3d71950..7ca59cb 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,8 @@ PolySTAR-Taproot-project/build PolySTAR-Taproot-project/Pipfile.lock -PolySTAR-Taproot-project/openocd.cfg + +openocd.local.cfg project.xml.log diff --git a/PolySTAR-Taproot-project/openocd.cfg b/PolySTAR-Taproot-project/openocd.cfg index da3fefb..26e4ef9 100644 --- a/PolySTAR-Taproot-project/openocd.cfg +++ b/PolySTAR-Taproot-project/openocd.cfg @@ -1,11 +1,3 @@ source [find interface/stlink.cfg] - -transport select hla_swd - -# increase working area to 128KB -set WORKAREASIZE 0x20000 - -source [find target/stm32f4x.cfg] - -reset_config none - +transport select swd +source [find target/stm32f4x.cfg] \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp index 8963731..8d767e9 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp @@ -6,4 +6,39 @@ namespace control::chassis::algorithms float k_pos; float k_vel; }; -} \ No newline at end of file +} + +#ifdef TARGET_HERO +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; +#endif + +#ifdef TARGET_SENTRY +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; +#endif + +#ifdef TARGET_STANDARD +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; +#endif + +#ifdef TARGET_ICRA +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; +#endif + +#ifdef TARGET_SPIN_TO_WIN +// defauilt lqr gains (standard) +static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; +static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; +#endif diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp index 9092d08..6842e09 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -1,5 +1,4 @@ #include "chassis_lqr.hpp" -#include "chassis_constants.hpp" #include // std::clamp namespace control @@ -52,9 +51,9 @@ std::array ChassisLqrController::update(float vx, float dvx, float vx_r return { clamp(fl), clamp(fr), clamp(bl), clamp(br) }; } -Gains2 ChassisLqrController::defaultGains(float dyn) +Gains2 ChassisLqrController::defaultGains() { - return { 1.5f * dyn, 0.1f }; + return { 1.5f, 0.1f }; } } // namespace chassis::algorithms diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp index dd1bfcb..aa1bc5f 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -33,7 +33,7 @@ class ChassisLqrController float w, float dw, float w_ref); private: - static Gains2 defaultGains(float dyn); + static Gains2 defaultGains(); private: float m_; diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp index 9e3e979..60ca53a 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp @@ -1,6 +1,4 @@ - #pragma once -#include "./algorithms/chassis_gains.hpp" /** * Chassis wheel velocity PID: A PD controller for chassis wheel RPM. The PID parameters for the * controller are listed below. @@ -83,40 +81,20 @@ static constexpr uint32_t CHASSIS_DEBUG_MESSAGE_DELAY_MS = 100; */ #ifdef TARGET_HERO static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; -// defauilt lqr gains (standard) -static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_SENTRY static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; -// defauilt lqr gains (standard) -static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_STANDARD static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; -// defauilt lqr gains (standard) -static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_ICRA static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; -// defauilt lqr gains (standard) -static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif #ifdef TARGET_SPIN_TO_WIN static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; -// defauilt lqr gains (standard) -static constexpr control::chassis::algorithms::Gains2 Kx = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Ky = {63.24f, 11.25f}; -static constexpr control::chassis::algorithms::Gains2 Kt = {15.81f, 2.45f}; #endif From b4f6ba5d33c0dde70f8cb1b01170a51a220f3c3f Mon Sep 17 00:00:00 2001 From: Glory Date: Fri, 20 Mar 2026 21:15:59 -0400 Subject: [PATCH 12/12] fix: original version of openocd.cfg --- PolySTAR-Taproot-project/openocd.cfg | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/PolySTAR-Taproot-project/openocd.cfg b/PolySTAR-Taproot-project/openocd.cfg index 26e4ef9..6dfcd39 100644 --- a/PolySTAR-Taproot-project/openocd.cfg +++ b/PolySTAR-Taproot-project/openocd.cfg @@ -1,3 +1,10 @@ source [find interface/stlink.cfg] -transport select swd -source [find target/stm32f4x.cfg] \ No newline at end of file + +transport select hla_swd + +# increase working area to 128KB +set WORKAREASIZE 0x20000 + +source [find target/stm32f4x.cfg] + +reset_config none