diff --git a/.gitignore b/.gitignore index db4726b2..7ca59cb6 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,9 @@ PolySTAR-Taproot-project/build PolySTAR-Taproot-project/Pipfile.lock + +openocd.local.cfg + project.xml.log .vscode/.cortex-debug.peripherals.state.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 19491371..c5d09648 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/openocd.cfg b/PolySTAR-Taproot-project/openocd.cfg index da3fefb0..6dfcd399 100644 --- a/PolySTAR-Taproot-project/openocd.cfg +++ b/PolySTAR-Taproot-project/openocd.cfg @@ -8,4 +8,3 @@ set WORKAREASIZE 0x20000 source [find target/stm32f4x.cfg] reset_config none - diff --git a/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp b/PolySTAR-Taproot-project/src/control/Icra/icra_control.cpp index 518f130f..672895cc 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/control/robot_config.hpp b/PolySTAR-Taproot-project/src/control/robot_config.hpp new file mode 100644 index 00000000..2bf6b9a7 --- /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/algorithms/chassis_gains.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp new file mode 100644 index 00000000..8d767e92 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_gains.hpp @@ -0,0 +1,44 @@ +#pragma once + +namespace control::chassis::algorithms +{ + struct Gains2 { + float k_pos; + float k_vel; + }; +} + +#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 new file mode 100644 index 00000000..6842e097 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp @@ -0,0 +1,60 @@ +#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) +{ + // 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_ = Kx; + Ky_ = Ky; + Kt_ = Kt; +} + +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) }; +} + +Gains2 ChassisLqrController::defaultGains() +{ + return { 1.5f, 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 00000000..aa1bc5fc --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp @@ -0,0 +1,52 @@ +#ifndef CHASSIS_LQR_HPP_ +#define CHASSIS_LQR_HPP_ +#include +#include "chassis_gains.hpp" +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: + /* 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); + + 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: + static Gains2 defaultGains(); + +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_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp index 337f7e96..60ca53a2 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 index 0bf9ec0c..b6c939ec 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp @@ -197,5 +197,4 @@ void ChassisSpin2WinSubsystem::sendCVUpdate() { } // namespace chassis -} // namespace control - +} // 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 index e4adb5ea..bfd2d8ae 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp @@ -146,4 +146,4 @@ class ChassisSpin2WinSubsystem : public tap::control::Subsystem } // namespace control -#endif // CHASSIS_SUBSYSTEM_HPP_ +#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 d3c7a63f..be5058a1 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" @@ -20,17 +22,54 @@ void ChassisSubsystem::initialize() backLeftMotor.initialize(); backRightMotor.initialize(); prevRampUpdate = tap::arch::clock::getTimeMilliseconds(); + prevControlUpdate = prevRampUpdate; } void ChassisSubsystem::refresh() { updateRpmSetpoints(); + // 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); + + // 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; + - 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(); + // 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, + w, dw, 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,41 +86,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); - 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); + (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; @@ -90,11 +123,18 @@ 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(); } 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); @@ -121,11 +161,6 @@ 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; } /* @@ -196,6 +231,22 @@ void ChassisSubsystem::sendCVUpdate() { drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&positionMessage), sizeof(positionMessage)); } +// 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 } // 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 aa116740..7e8de300 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -1,14 +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" @@ -16,6 +19,7 @@ namespace control { namespace chassis { + /** * A bare bones Subsystem for interacting with a 4 wheeled chassis. */ @@ -32,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. @@ -44,16 +50,22 @@ 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( + CHASSIS_MASS_KG, + CHASSIS_YAW_INERTIA_KGM2, + MAX_CURRENT_OUTPUT, + rpmScaleFactor +); } ChassisSubsystem(const ChassisSubsystem &other) = delete; @@ -68,7 +80,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); @@ -79,12 +91,26 @@ 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;} + 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); + 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; @@ -97,23 +123,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; @@ -133,7 +169,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; @@ -142,9 +178,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 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 00000000..74966a8b --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.cpp @@ -0,0 +1,43 @@ +#include "turret_lqr.hpp" +#include + +namespace turret::algorithms +{ +TurretLqrController::TurretLqrController(float panInertia, + float tiltInertia, + float motorOutputMax, + float axisToMotorScale) + : I_pan_(panInertia), + I_tilt_(tiltInertia), + maxOut_(motorOutputMax), + scale_(axisToMotorScale) +{ + // Gains LQR optimaux calcules avec Simulink CARE solver + Ktilt_ = {31.62f, 1.61f}; + Kpan_ = {31.62f, 3.62f}; +} + +void TurretLqrController::setGains(const Gains2& Kpan, const Gains2& Ktilt) +{ + Kpan_ = Kpan; + Ktilt_ = Ktilt; +} + +std::array TurretLqrController::update(float panAngle, float panRate, float panRef, + float tiltAngle, float tiltRate, float tiltRef) +{ + const float errPan = panAngle - panRef; + const float errTilt = tiltAngle - tiltRef; + + 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(uPan), clamp(uTilt) }; +} + +TurretLqrController::Gains2 TurretLqrController::defaultGains(float dyn) +{ + 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 new file mode 100644 index 00000000..239093ef --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/turret/algorithms/turret_lqr.hpp @@ -0,0 +1,41 @@ +#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: + struct Gains2 { float k_pos; float k_vel; }; + + TurretLqrController(float panInertia, + float tiltInertia, + float motorOutputMax = 8000.0f, + float axisToMotorScale = 650.0f); + + void setGains(const Gains2& Kpan, const Gains2& Ktilt); + + // 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); + +private: + static Gains2 defaultGains(float dyn); + +private: + float I_pan_; + float I_tilt_; + float maxOut_; + float scale_; + + Gains2 Kpan_{}; + Gains2 Ktilt_{}; +}; +} // namespace turret::algorithms + +#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 71fbae96..643a2866 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp @@ -46,10 +46,22 @@ 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 = 0.017453293f; +static constexpr float RPM_TO_RAD_S = 0.104719755f; + /** * 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.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 293670ea..73433277 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -59,30 +59,57 @@ void TurretSubsystem::refresh() { Run yaw controller and update motor output. */ void TurretSubsystem::runYawController(uint32_t dt) { - float error = yawDesiredPos - yawMotor->getEncoderWrapped(); + // 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) { - // 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); + error = error - DjiMotor::ENC_RESOLUTION * getSign(error); } + + // Check how fast the turret is currently spinning. int16_t currentRPM = yawMotor->getShaftRPM(); - - cascadedYawController.update(error, currentRPM, dt); - - yawMotor->setDesiredOutput(cascadedYawController.getOutput()); + + // 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]); } -/* - Run pitch controller and update motor output. -*/ void TurretSubsystem::runPitchController(uint32_t dt) { - float error = pitchDesiredPos - pitchMotor.getEncoderWrapped(); + // 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(); - - cascadedPitchController.update(error, currentRPM, dt); - - pitchMotor.setDesiredOutput(cascadedPitchController.getOutput()); + + // 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]); } /* @@ -167,59 +194,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 856f9184..f39e9898 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,15 +33,18 @@ 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, + /*motorOutputMax*/ 8000.0f, + /*axisToMotorScale*/ 350.0f), yawDesiredPos(YAW_NEUTRAL_POS), pitchDesiredPos(PITCH_NEUTRAL_POS) { } TurretSubsystem(const TurretSubsystem &other) = delete; - TurretSubsystem &operator=(const TurretSubsystem &other) = delete; ~TurretSubsystem() = default; @@ -73,10 +78,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,14 +93,20 @@ 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; + + // Linear Quadratic Regulator for optimal turret stability. + TurretLqrController lqrTurret; // Position setpoints for turret, in encoder ticks float yawDesiredPos; float pitchDesiredPos; + bool yawInDeadzone_ = false; + bool pitchInDeadzone_ = false; + // Time variables for fixed rate tasks uint32_t prevDebugUpdate; uint32_t prevControllerUpdate;