Refactor/pid to lqr

.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

.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
+}

PolySTAR-Taproot-project/openocd.cfg

@@ -8,4 +8,3 @@ set WORKAREASIZE 0x20000
 source [find target/stm32f4x.cfg]
 
 reset_config none
-

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 --------------------------*/

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;
+};

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

PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.cpp

@@ -0,0 +1,60 @@
+#include "chassis_lqr.hpp"
+#include <algorithm>  // 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<float,4> 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

PolySTAR-Taproot-project/src/subsystems/chassis/algorithms/chassis_lqr.hpp

@@ -0,0 +1,52 @@
+#ifndef CHASSIS_LQR_HPP_
+#define CHASSIS_LQR_HPP_
+#include <array>
+#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<float,4> 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_

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;
 

PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.cpp

@@ -197,5 +197,4 @@ void ChassisSpin2WinSubsystem::sendCVUpdate() {
 
 }  // namespace chassis
 
-}  // namespace control
-
+}  // namespace control

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_