adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Loaded new mass and inertial characteristics into sim, ported sim arch into arduino code

Browse Source
This commit is contained in:
pulipakaa24
2026-02-26 12:45:27 -06:00
parent 725f227943
commit 010d997eef
16 changed files with 450 additions and 473 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
AdditiveControlCode/AdditiveControlCode.ino
View File

@@ -4,43 +4,40 @@
#include "ADC.hpp" #include "ADC.hpp"
#include "FastPWM.hpp" #include "FastPWM.hpp"
// K, Ki, Kd Constants // ── PID Gains (Kp, Ki, Kd) ──────────────────────────────────
Constants repelling = {250, 0, 1000}; // Height loop: controls average gap → additive PWM on all coils
Constants attracting = {250, 0, 1000}; PIDGains heightGains = { 100.0f, 0.0f, 8.0f };
Constants RollLeftUp = {0, 0, 100}; // Roll loop: corrects left/right tilt → differential L/R
Constants RollLeftDown = {0, 0, 100}; PIDGains rollGains = { 0.6f, 0.0f, -0.1f };
Constants RollFrontUp = {0, 0, 500}; // Pitch loop: corrects front/back tilt → differential F/B
Constants RollFrontDown = {0, 0, 500}; PIDGains pitchGains = { 50.0f, 0.0f, 1.9f };
// Reference values for average dist, // ── Reference ────────────────────────────────────────────────
float avgRef = 11.0; // TBD: what is our equilibrium height with this testrig? float avgRef = 12.36f; // Target gap height (mm) — 9.4 kg equilibrium
float LRDiffRef = -2.0; // TBD: what is our left-right balance equilibrium? Positive -> left is above right
float FBDiffRef = 0.0; // TBD: what is front-back balance equilibrium? Positive -> front above back.
// Might be useful for things like jitter or lag. // ── Feedforward ──────────────────────────────────────────────
#define sampling_rate 1000 // Hz bool useFeedforward = true; // Set false to disable feedforward LUT
// EMA filter alpha value (all sensors use same alpha) // ── Sampling ─────────────────────────────────────────────────
#define alphaVal 0.3f #define SAMPLING_RATE 200 // Hz (controller tick rate)
// ── EMA filter alpha (all sensors) ───────────────────────────
#define ALPHA_VAL 1.0f
// ═══════════════════════════════════════════════════════════════
// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE. // ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
// ═══════════════════════════════════════════════════════════════
unsigned long tprior; unsigned long tprior;
unsigned int tDiffMicros; unsigned int tDiffMicros;
FullConsts fullConsts = { FullController controller(indL, indR, indF, indB,
{repelling, attracting}, heightGains, rollGains, pitchGains,
{RollLeftDown, RollLeftUp}, avgRef, useFeedforward);
{RollFrontDown, RollFrontUp}
};
FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef); const int dt_micros = 1000000 / SAMPLING_RATE;
const int dt_micros = 1e6/sampling_rate;
#define LEV_ON
int ON = 0; int ON = 0;
@@ -49,10 +46,10 @@ void setup() {
setupADC(); setupADC();
setupFastPWM(); setupFastPWM();
indL.alpha = alphaVal; indL.alpha = ALPHA_VAL;
indR.alpha = alphaVal; indR.alpha = ALPHA_VAL;
indF.alpha = alphaVal; indF.alpha = ALPHA_VAL;
indB.alpha = alphaVal; indB.alpha = ALPHA_VAL;
tprior = micros(); tprior = micros();
@@ -71,91 +68,61 @@ void loop() {
String cmd = Serial.readStringUntil('\n'); String cmd = Serial.readStringUntil('\n');
cmd.trim(); cmd.trim();
// Check if it's a reference update command (format: REF,avgRef,lrDiffRef,fbDiffRef) // REF,avgRef — update target gap height
if (cmd.startsWith("REF,")) { if (cmd.startsWith("REF,")) {
int firstComma = cmd.indexOf(','); float newRef = cmd.substring(4).toFloat();
int secondComma = cmd.indexOf(',', firstComma + 1); avgRef = newRef;
int thirdComma = cmd.indexOf(',', secondComma + 1); controller.updateReference(avgRef);
Serial.print("Updated Ref: ");
if (firstComma > 0 && secondComma > 0 && thirdComma > 0) { Serial.println(avgRef);
float newAvgRef = cmd.substring(firstComma + 1, secondComma).toFloat();
float newLRDiffRef = cmd.substring(secondComma + 1, thirdComma).toFloat();
float newFBDiffRef = cmd.substring(thirdComma + 1).toFloat();
avgRef = newAvgRef;
LRDiffRef = newLRDiffRef;
FBDiffRef = newFBDiffRef;
controller.updateReferences(avgRef, LRDiffRef, FBDiffRef);
Serial.print("Updated References: Avg=");
Serial.print(avgRef);
Serial.print(", LR=");
Serial.print(LRDiffRef);
Serial.print(", FB=");
Serial.println(FBDiffRef);
} }
} // PID,loop,kp,ki,kd — update gains (loop: 0=height, 1=roll, 2=pitch)
// Check if it's a PID tuning command (format: PID,mode,kp,ki,kd)
else if (cmd.startsWith("PID,")) { else if (cmd.startsWith("PID,")) {
int firstComma = cmd.indexOf(','); int c1 = cmd.indexOf(',');
int secondComma = cmd.indexOf(',', firstComma + 1); int c2 = cmd.indexOf(',', c1 + 1);
int thirdComma = cmd.indexOf(',', secondComma + 1); int c3 = cmd.indexOf(',', c2 + 1);
int fourthComma = cmd.indexOf(',', thirdComma + 1); int c4 = cmd.indexOf(',', c3 + 1);
if (firstComma > 0 && secondComma > 0 && thirdComma > 0 && fourthComma > 0) { if (c1 > 0 && c2 > 0 && c3 > 0 && c4 > 0) {
int mode = cmd.substring(firstComma + 1, secondComma).toInt(); int loop = cmd.substring(c1 + 1, c2).toInt();
float kp = cmd.substring(secondComma + 1, thirdComma).toFloat(); float kp = cmd.substring(c2 + 1, c3).toFloat();
float ki = cmd.substring(thirdComma + 1, fourthComma).toFloat(); float ki = cmd.substring(c3 + 1, c4).toFloat();
float kd = cmd.substring(fourthComma + 1).toFloat(); float kd = cmd.substring(c4 + 1).toFloat();
Constants newConst = {kp, ki, kd}; PIDGains g = { kp, ki, kd };
// Mode mapping: switch (loop) {
// 0: Repelling case 0:
// 1: Attracting heightGains = g;
// 2: RollLeftDown controller.updateHeightPID(g);
// 3: RollLeftUp Serial.println("Updated Height PID");
// 4: RollFrontDown
// 5: RollFrontUp
switch(mode) {
case 0: // Repelling
repelling = newConst;
controller.updateAvgPID(repelling, attracting);
Serial.println("Updated Repelling PID");
break; break;
case 1: // Attracting case 1:
attracting = newConst; rollGains = g;
controller.updateAvgPID(repelling, attracting); controller.updateRollPID(g);
Serial.println("Updated Attracting PID"); Serial.println("Updated Roll PID");
break; break;
case 2: // RollLeftDown case 2:
RollLeftDown = newConst; pitchGains = g;
controller.updateLRPID(RollLeftDown, RollLeftUp); controller.updatePitchPID(g);
Serial.println("Updated RollLeftDown PID"); Serial.println("Updated Pitch PID");
break;
case 3: // RollLeftUp
RollLeftUp = newConst;
controller.updateLRPID(RollLeftDown, RollLeftUp);
Serial.println("Updated RollLeftUp PID");
break;
case 4: // RollFrontDown
RollFrontDown = newConst;
controller.updateFBPID(RollFrontDown, RollFrontUp);
Serial.println("Updated RollFrontDown PID");
break;
case 5: // RollFrontUp
RollFrontUp = newConst;
controller.updateFBPID(RollFrontDown, RollFrontUp);
Serial.println("Updated RollFrontUp PID");
break; break;
default: default:
Serial.println("Invalid mode"); Serial.println("Invalid loop (0=height, 1=roll, 2=pitch)");
break; break;
} }
} }
} else { }
// Original control on/off command // FF,0 or FF,1 — toggle feedforward
else if (cmd.startsWith("FF,")) {
bool en = (cmd.charAt(3) != '0');
useFeedforward = en;
controller.setFeedforward(en);
Serial.print("Feedforward: ");
Serial.println(en ? "ON" : "OFF");
}
else {
// Original on/off command (any char except '0' turns on)
controller.outputOn = (cmd.charAt(0) != '0'); controller.outputOn = (cmd.charAt(0) != '0');
} }
} }
@@ -166,11 +133,7 @@ void loop() {
controller.update(); controller.update();
controller.report(); controller.report();
controller.sendOutputs(); controller.sendOutputs();
// this and the previous line can be switched if you want the PWMs to display 0 when controller off.
tprior = micros(); // maybe we have to move this line to before the update commands? tprior = micros();
// since the floating point arithmetic may take a while...
} }
//Serial.println(telapsed);
} }

28
RL Testing/Function Fitting.ipynb
View File

@@ -2189,14 +2189,14 @@
"id": "21babeb3", "id": "21babeb3",
"metadata": {}, "metadata": {},
"source": [ "source": [
"### Now, let's find the equilibrium height for our pod, given mass of 5.8 kg. \n", "### Now, let's find the equilibrium height for our pod, given mass of 9.4 kg. \n",
"\n", "\n",
"5.8 kg * 9.81 $m/s^2$ = 56.898 N" "9.4 kg * 9.81 $m/s^2$ = 56.898 N"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 50, "execution_count": 1,
"id": "badbc379", "id": "badbc379",
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
@@ -2204,23 +2204,27 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Loading maglev model from /Users/adipu/Documents/guadaloop_lev_control/RL Testing/maglev_model.pkl...\n",
"Model loaded. Degree: 6\n",
"Force R2: 1.0000\n",
"Torque R2: 0.9999\n",
"======================================================================\n", "======================================================================\n",
"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\n", "EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\n",
"======================================================================\n", "======================================================================\n",
"Pod mass: 5.8 kg\n", "Pod mass: 9.4 kg\n",
"Total weight: 56.898 N\n", "Total weight: 92.214 N\n",
"Target force per yoke: 28.449 N\n", "Target force per yoke: 46.107 N\n",
"Parameters: currL = 0 A, currR = 0 A, roll = 0°\n", "Parameters: currL = 0 A, currR = 0 A, roll = 0°\n",
"\n", "\n",
"using scipy.optimize.fsolve:\n", "using scipy.optimize.fsolve:\n",
" Gap: 16.491742 mm → Force: 28.449000 N\n", " Gap: 11.857530 mm → Force: 46.107000 N\n",
"\n" "\n"
] ]
} }
], ],
"source": [ "source": [
"# The following was generated by AI - see [13]\n", "# The following was generated by AI - see [13]\n",
"# Find equilibrium gap height for 5.8 kg pod using polynomial root finding\n", "# Find equilibrium gap height for 9.4 kg pod using polynomial root finding\n",
"import numpy as np\n", "import numpy as np\n",
"from maglev_predictor import MaglevPredictor\n", "from maglev_predictor import MaglevPredictor\n",
"from scipy.optimize import fsolve\n", "from scipy.optimize import fsolve\n",
@@ -2228,15 +2232,15 @@
"# Initialize predictor\n", "# Initialize predictor\n",
"predictor = MaglevPredictor()\n", "predictor = MaglevPredictor()\n",
"\n", "\n",
"# Target force for 5.8 kg pod (total force = weight)\n", "# Target force for 9.4 kg pod (total force = weight)\n",
"# Since we have TWO yokes (front and back), each produces this force\n", "# Since we have TWO yokes (front and back), each produces this force\n",
"target_force_per_yoke = 5.8 * 9.81 / 2 # 28.449 N per yoke\n", "target_force_per_yoke = 9.4 * 9.81 / 2 # 28.449 N per yoke\n",
"\n", "\n",
"print(\"=\" * 70)\n", "print(\"=\" * 70)\n",
"print(\"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\")\n", "print(\"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\")\n",
"print(\"=\" * 70)\n", "print(\"=\" * 70)\n",
"print(f\"Pod mass: 5.8 kg\")\n", "print(f\"Pod mass: 9.4 kg\")\n",
"print(f\"Total weight: {5.8 * 9.81:.3f} N\")\n", "print(f\"Total weight: {9.4 * 9.81:.3f} N\")\n",
"print(f\"Target force per yoke: {target_force_per_yoke:.3f} N\")\n", "print(f\"Target force per yoke: {target_force_per_yoke:.3f} N\")\n",
"print(f\"Parameters: currL = 0 A, currR = 0 A, roll = 0°\")\n", "print(f\"Parameters: currL = 0 A, currR = 0 A, roll = 0°\")\n",
"print()\n", "print()\n",

2
RL Testing/archive_not_used/maglev_state_space.py
View File

@@ -217,7 +217,7 @@ class MaglevStateSpace:
""" """
def __init__(self, linearizer, def __init__(self, linearizer,
mass=5.8, mass=9.4,
I_roll=0.0192942414, # Ixx from pod.xml [kg·m²] I_roll=0.0192942414, # Ixx from pod.xml [kg·m²]
I_pitch=0.130582305, # Iyy from pod.xml [kg·m²] I_pitch=0.130582305, # Iyy from pod.xml [kg·m²]
coil_R=1.1, # from MagLevCoil in lev_pod_env.py coil_R=1.1, # from MagLevCoil in lev_pod_env.py

188
RL Testing/lev_PID.ipynb
View File

File diff suppressed because one or more lines are too long

8
RL Testing/lev_pod_env.py
View File

@@ -8,7 +8,7 @@ from datetime import datetime
from mag_lev_coil import MagLevCoil from mag_lev_coil import MagLevCoil
from maglev_predictor import MaglevPredictor from maglev_predictor import MaglevPredictor
TARGET_GAP = 16.491741 / 1000 # target gap height for 5.8 kg pod in meters TARGET_GAP = 11.86 / 1000 # target gap height for 9.4 kg pod in meters
class LevPodEnv(gym.Env): class LevPodEnv(gym.Env):
def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000, disturbance_force_std=0.0, def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000, disturbance_force_std=0.0,
@@ -385,8 +385,8 @@ class LevPodEnv(gym.Env):
obs = self._get_obs() obs = self._get_obs()
# --- 8. Calculate Reward --- # --- 8. Calculate Reward ---
# Goal: Hover at target gap (16.5mm), minimize roll/pitch, minimize power # Goal: Hover at target gap (11.8mm), minimize roll/pitch, minimize power
target_gap = TARGET_GAP # 16.5mm in meters target_gap = TARGET_GAP # 11.8mm in meters
avg_gap = (avg_gap_front + avg_gap_back) / 2 avg_gap = (avg_gap_front + avg_gap_back) / 2
gap_error = abs(avg_gap - target_gap) gap_error = abs(avg_gap - target_gap)
@@ -656,7 +656,7 @@ class LevPodEnv(gym.Env):
t = {k: np.array(v) for k, v in self._telemetry.items()} t = {k: np.array(v) for k, v in self._telemetry.items()}
time = t['time'] time = t['time']
target_mm = TARGET_GAP * 1000 target_mm = TARGET_GAP * 1000
weight = 5.8 * 9.81 # Pod weight in N weight = 9.4 * 9.81 # Pod weight in N
fig, axes = plt.subplots(4, 1, figsize=(12, 14), sharex=True) fig, axes = plt.subplots(4, 1, figsize=(12, 14), sharex=True)
fig.suptitle(f'Simulation Telemetry | gap₀={self.initial_gap_mm}mm target={target_mm:.1f}mm', fig.suptitle(f'Simulation Telemetry | gap₀={self.initial_gap_mm}mm target={target_mm:.1f}mm',

2
RL Testing/optuna_pid_tune.py
View File

@@ -27,7 +27,7 @@ _SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
# Default optimization config: long enough to see late instability (~8s+), not so long that optimizer goes ultra-conservative # Default optimization config: long enough to see late instability (~8s+), not so long that optimizer goes ultra-conservative
DEFAULT_MAX_STEPS = 1500 DEFAULT_MAX_STEPS = 1500
DEFAULT_INITIAL_GAPS_MM = [12.0, 18.0] # Two conditions for robustness DEFAULT_INITIAL_GAPS_MM = [8.0, 15.0] # Two conditions for robustness (bracket 11.86mm target)
DEFAULT_N_TRIALS = 200 DEFAULT_N_TRIALS = 200
DEFAULT_TIMEOUT_S = 3600 DEFAULT_TIMEOUT_S = 3600
TARGET_GAP_MM = TARGET_GAP * 1000 TARGET_GAP_MM = TARGET_GAP * 1000

16
RL Testing/pid_best_params.json
View File

@@ -1,11 +1,11 @@
{ {
"height_kp": 80.05607483893696, "height_kp": 100.962395560374814,
"height_ki": 0, "height_ki": 0.0,
"height_kd": 7.09266287860531, "height_kd": 8.1738092637166575,
"roll_kp": -0.600856607986966, "roll_kp": 0.600856607986966,
"roll_ki": 0, "roll_ki": 0.0,
"roll_kd": -0.1, "roll_kd": -0.1,
"pitch_kp": 50.3415835489009009, "pitch_kp": 50.0114708799258271,
"pitch_ki": 0.02319184022898008, "pitch_ki": 0,
"pitch_kd": 0.017632648760979346 "pitch_kd": 1.8990306608320433
} }

18
RL Testing/pid_best_params_1500.json
View File

@@ -1,11 +1,11 @@
{ {
"height_kp": 112.05607483893696, "height_kp": 61.34660453658844,
"height_ki": 1.3231206601751908, "height_ki": 5.337339965349835,
"height_kd": 14.09266287860531, "height_kd": 12.13071554123404,
"roll_kp": 3.600856607986966, "roll_kp": 5.838881924776812,
"roll_ki": 0.10314962498074487, "roll_ki": 0.11040111644948386,
"roll_kd": 0.013792861414632316, "roll_kd": 0.0401383180893775,
"pitch_kp": 0.1415835489009009, "pitch_kp": 0.10114651080341679,
"pitch_ki": 0.02319184022898008, "pitch_ki": 0.06948994902747452,
"pitch_kd": 0.017632648760979346 "pitch_kd": 0.16948986671689478
} }

18
RL Testing/pid_best_params_3000.json
View File

@@ -1,11 +1,11 @@
{ {
"height_kp": 7.886377342450826, "height_kp": 6.79952358593656,
"height_ki": 0.5011631292801424, "height_ki": 2.7199339229214856,
"height_kd": 6.281854346104332, "height_kd": 12.166576111298163,
"roll_kp": 0.5834114273785317, "roll_kp": 1.7073146141313746,
"roll_ki": 4.958217971237515, "roll_ki": 0.026221209129363342,
"roll_kd": 0.020111264395006163, "roll_kd": 0.018353603438859525,
"pitch_kp": 1.7442062254360877, "pitch_kp": 1.2333241666054757,
"pitch_ki": 0.03376218829632256, "pitch_ki": 0.03544610305322942,
"pitch_kd": 2.7713413800138587 "pitch_kd": 0.3711162924888727
} }

18
RL Testing/pid_best_params_6000.json
View File

@@ -1,11 +1,11 @@
{ {
"height_kp": 6.387288593598476, "height_kp": 5.962395560374814,
"height_ki": 3.8692474098125285, "height_ki": 0.5381137743695537,
"height_kd": 8.326592570264028, "height_kd": 1.1738092637166575,
"roll_kp": 2.5896160034319817, "roll_kp": 0.48249575996443006,
"roll_ki": 0.26391970508131646, "roll_ki": 0.016923890033141546,
"roll_kd": 0.10133339760273202, "roll_kd": 0.013609429509460135,
"pitch_kp": 0.5612446354489615, "pitch_kp": 0.10114708799258271,
"pitch_ki": 0.014113041384510265, "pitch_ki": 1.906050976563914,
"pitch_kd": 1.100684970603034 "pitch_kd": 1.8990306608320433
} }

2
RL Testing/pid_simulation.py
View File

@@ -14,7 +14,7 @@ _FF_PWM_LUT = None
def build_feedforward_lut( def build_feedforward_lut(
pod_mass: float = 5.8, pod_mass: float = 9.4,
coil_r: float = 1.1, coil_r: float = 1.1,
v_supply: float = 12.0, v_supply: float = 12.0,
gap_min: float = 3.0, gap_min: float = 3.0,

12
RL Testing/pod.xml
View File

@@ -2,8 +2,8 @@
<robot name="lev_pod"> <robot name="lev_pod">
<link name="base_link"> <link name="base_link">
<inertial> <inertial>
<mass value="5.8"/> <mass value="9.4"/>
<inertia ixx="0.0192942414" ixy="0.0" ixz="0.0" iyy="0.130582305" iyz="0.0" izz="0.13760599326"/> <inertia ixx="0.03162537654" ixy="0.0" ixz="0.0" iyy="0.21929017831" iyz="0.0" izz="0.21430205089"/>
</inertial> </inertial>
<collision> <collision>
@@ -13,19 +13,19 @@
Bolts Bolts
<collision> <collision>
<origin rpy="0 0 0" xyz="0.285 0.03 0.09585"/> <origin rpy="0 0 0" xyz="0.285 0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry> <geometry><box size="0.01 0.01 0.01"/></geometry>
</collision> </collision>
<collision> <collision>
<origin rpy="0 0 0" xyz="0.285 -0.03 0.09585"/> <origin rpy="0 0 0" xyz="0.285 -0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry> <geometry><box size="0.01 0.01 0.01"/></geometry>
</collision> </collision>
<collision> <collision>
<origin rpy="0 0 0" xyz="-0.285 0.03 0.09585"/> <origin rpy="0 0 0" xyz="-0.285 0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry> <geometry><box size="0.01 0.01 0.01"/></geometry>
</collision> </collision>
<collision> <collision>
<origin rpy="0 0 0" xyz="-0.285 -0.03 0.09585"/> <origin rpy="0 0 0" xyz="-0.285 -0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry> <geometry><box size="0.01 0.01 0.01"/></geometry>
</collision> </collision>

18
RL Testing/temp.json
View File

@@ -1,11 +1,11 @@
{ {
"height_kp": 6.387288593598476, "height_kp": 80.05607483893696,
"height_ki": 3.8692474098125285, "height_ki": 0,
"height_kd": 8.326592570264028, "height_kd": 7.09266287860531,
"roll_kp": 2.5896160034319817, "roll_kp": -0.600856607986966,
"roll_ki": 0.26391970508131646, "roll_ki": 0,
"roll_kd": 0.10133339760273202, "roll_kd": -0.1,
"pitch_kp": 0.5612446354489615, "pitch_kp": 50.3415835489009009,
"pitch_ki": 0.014113041384510265, "pitch_ki": 0.02319184022898008,
"pitch_kd": 1.100684970603034 "pitch_kd": 0.017632648760979346
} }

265
lib/Controller.cpp
View File

@@ -1,32 +1,132 @@
#include "Controller.hpp" #include "Controller.hpp"
#include <Arduino.h> #include <Arduino.h>
#include <math.h>
// CONTROLLER CONSTANTS // ── Integral windup limit ────────────────────────────────────
float MAX_INTEGRAL_TERM = 1e4; static const float MAX_INTEGRAL = 1e4f;
// ── Feedforward LUT in PROGMEM ───────────────────────────────
// Generated by gen_ff_lut.py (pod 9.4 kg, R=1.1Ω, V=12V, 3–20 mm)
// Positive = repelling, Negative = attracting
static const int16_t FF_PWM_LUT[FF_LUT_SIZE] PROGMEM = {
238, 238, 238, 238, 238, 238, 238, 238,
238, 238, 238, 238, 238, 238, 238, 238,
238, 238, 234, 219, 204, 188, 172, 157,
141, 125, 109, 93, 77, 61, 45, 29,
13, -3, -19, -35, -51, -67, -84, -100,
-116, -133, -150, -166, -183, -200, -217, -234,
-250, -250, -250, -250, -250, -250, -250, -250,
-250, -250, -250, -250, -250, -250, -250, -250
};
// ── Constructor ──────────────────────────────────────────────
FullController::FullController(
IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
PIDGains hGains, PIDGains rGains, PIDGains pGains,
float avgRef, bool useFeedforward)
: Left(l), Right(r), Front(f), Back(b),
heightGains(hGains), rollGains(rGains), pitchGains(pGains),
heightErr({0,0,0}), rollErr({0,0,0}), pitchErr({0,0,0}),
AvgRef(avgRef), avg(0), ffEnabled(useFeedforward),
oor(false), outputOn(false),
FLPWM(0), BLPWM(0), FRPWM(0), BRPWM(0)
{}
// ── Main update (called each control tick) ───────────────────
void FullController::update() { void FullController::update() {
// 1. Read all sensors (updates mmVal, oor)
Left.readMM(); Left.readMM();
Right.readMM(); Right.readMM();
Front.readMM(); Front.readMM();
Back.readMM(); // read and update dists/oor for all sensors. Back.readMM();
oor = Left.oor || Right.oor || Front.oor || Back.oor; oor = Left.oor || Right.oor || Front.oor || Back.oor;
avgControl(); // 2. Compute average gap (mm)
LRControl(); // run pwm functions. avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
FBControl();
FLPWM = constrain(avgPWM + LDiffPWM + FDiffPWM, -CAP, CAP); // 3. Feedforward: base PWM from equilibrium LUT
BLPWM = constrain(avgPWM + LDiffPWM + BDiffPWM, -CAP, CAP); int16_t ffPWM = ffEnabled ? feedforward(avg) : 0;
FRPWM = constrain(avgPWM + RDiffPWM + FDiffPWM, -CAP, CAP);
BRPWM = constrain(avgPWM + RDiffPWM + BDiffPWM, -CAP, CAP);
// FLPWM = avgPWM; // 4. Height PID: error = reference - average gap
// BLPWM = avgPWM; float heightE = AvgRef - avg;
// FRPWM = avgPWM; heightErr.eDiff = heightE - heightErr.e;
// BRPWM = avgPWM; if (!oor) {
heightErr.eInt += heightE;
heightErr.eInt = constrain(heightErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
}
heightErr.e = heightE;
int16_t heightPWM = pidCompute(heightGains, heightErr, CAP);
// 5. Roll PID: angle-based error (matches simulation)
// roll_angle = asin((left - right) / y_distance)
// error = -roll_angle (drive roll toward zero)
float rollRatio = (Left.mmVal - Right.mmVal) / Y_DISTANCE_MM;
rollRatio = constrain(rollRatio, -1.0f, 1.0f);
float rollAngle = asinf(rollRatio);
float rollE = -rollAngle;
rollErr.eDiff = rollE - rollErr.e;
if (!oor) {
rollErr.eInt += rollE;
rollErr.eInt = constrain(rollErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
}
rollErr.e = rollE;
int16_t rollPWM = pidCompute(rollGains, rollErr, CAP / 2);
// 6. Pitch PID: angle-based error (matches simulation)
// pitch_angle = asin((back - front) / x_distance)
// error = -pitch_angle (drive pitch toward zero)
float pitchRatio = (Back.mmVal - Front.mmVal) / X_DISTANCE_MM;
pitchRatio = constrain(pitchRatio, -1.0f, 1.0f);
float pitchAngle = asinf(pitchRatio);
float pitchE = -pitchAngle;
pitchErr.eDiff = pitchE - pitchErr.e;
if (!oor) {
pitchErr.eInt += pitchE;
pitchErr.eInt = constrain(pitchErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
}
pitchErr.e = pitchE;
int16_t pitchPWM = pidCompute(pitchGains, pitchErr, CAP / 2);
// 7. Mix outputs (same sign convention as simulation)
// FL: ff + height - roll - pitch
// FR: ff + height + roll - pitch
// BL: ff + height - roll + pitch
// BR: ff + height + roll + pitch
FLPWM = constrain(ffPWM + heightPWM - rollPWM - pitchPWM, -CAP, CAP);
FRPWM = constrain(ffPWM + heightPWM + rollPWM - pitchPWM, -CAP, CAP);
BLPWM = constrain(ffPWM + heightPWM - rollPWM + pitchPWM, -CAP, CAP);
BRPWM = constrain(ffPWM + heightPWM + rollPWM + pitchPWM, -CAP, CAP);
} }
// ── PID compute (single symmetric set of gains) ─────────────
int16_t FullController::pidCompute(PIDGains& gains, PIDState& state, float maxOutput) {
if (oor) return 0;
float out = gains.kp * state.e
+ gains.ki * state.eInt
+ gains.kd * state.eDiff;
return (int16_t)constrain(out, -maxOutput, maxOutput);
}
// ── Feedforward: linearly interpolate PROGMEM LUT ────────────
int16_t FullController::feedforward(float gapMM) {
if (gapMM <= FF_GAP_MIN) return (int16_t)pgm_read_word(&FF_PWM_LUT[0]);
if (gapMM >= FF_GAP_MAX) return (int16_t)pgm_read_word(&FF_PWM_LUT[FF_LUT_SIZE - 1]);
float idx_f = (gapMM - 1.0 - FF_GAP_MIN) / FF_GAP_STEP;
uint8_t idx = (uint8_t)idx_f;
if (idx >= FF_LUT_SIZE - 1) idx = FF_LUT_SIZE - 2;
int16_t v0 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx]);
int16_t v1 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx + 1]);
float frac = idx_f - (float)idx;
return (int16_t)(v0 + frac * (v1 - v0));
}
// ── Zero all PWMs ────────────────────────────────────────────
void FullController::zeroPWMs() { void FullController::zeroPWMs() {
FLPWM = 0; FLPWM = 0;
BLPWM = 0; BLPWM = 0;
@@ -34,131 +134,48 @@ void FullController::zeroPWMs() {
BRPWM = 0; BRPWM = 0;
} }
// ── Send PWM values to hardware ──────────────────────────────
void FullController::sendOutputs() { void FullController::sendOutputs() {
if (!outputOn) { if (!outputOn) {
zeroPWMs(); zeroPWMs();
} }
// The following assumes 0 direction drives repulsion and 1 direction drives // Direction: LOW = repelling (positive PWM), HIGH = attracting (negative PWM)
// attraction. Using direct register writes to maintain fast PWM mode set by // Using direct register writes for fast PWM mode set by setupFastPWM()
// setupFastPWM()
digitalWrite(dirFL, FLPWM < 0); digitalWrite(dirFL, FLPWM < 0);
OCR2A = abs(FLPWM); // Pin 11 -> Timer 2A OCR2A = abs(FLPWM); // Pin 11 → Timer 2A
digitalWrite(dirBL, BLPWM < 0); digitalWrite(dirBL, BLPWM < 0);
OCR1A = abs(BLPWM); // Pin 9 -> Timer 1A OCR1A = abs(BLPWM); // Pin 9 → Timer 1A
digitalWrite(dirFR, FRPWM < 0); digitalWrite(dirFR, FRPWM < 0);
OCR2B = abs(FRPWM); // Pin 3 -> Timer 2B OCR2B = abs(FRPWM); // Pin 3 → Timer 2B
digitalWrite(dirBR, BRPWM < 0); digitalWrite(dirBR, BRPWM < 0);
OCR1B = abs(BRPWM); // Pin 10 -> Timer 1B OCR1B = abs(BRPWM); // Pin 10 → Timer 1B
}
void FullController::avgControl() {
avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
float eCurr = AvgRef - avg;
avgError.eDiff = eCurr - avgError.e;
if (!oor) {
avgError.eInt += eCurr;
avgError.eInt =
constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
}
avgError.e = eCurr;
avgPWM = pwmFunc(avgConsts, avgError);
}
void FullController::LRControl() {
float diff = Right.mmVal - Left.mmVal; // how far above the right is the left?
float eCurr = diff - LRDiffRef; // how different is that from the reference?
// positive -> Left repels, Right attracts.
K_MAP rConsts = {
LConsts.attracting,
LConsts.repelling}; // apply attracting to repelling and vice versa.
LRDiffErr.eDiff = eCurr - LRDiffErr.e;
if (!oor) {
LRDiffErr.eInt += eCurr;
LRDiffErr.eInt =
constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
}
LRDiffErr.e = eCurr;
LDiffPWM = pwmFunc(LConsts, LRDiffErr);
RDiffPWM = -pwmFunc(rConsts, LRDiffErr);
}
void FullController::FBControl() {
float diff = Back.mmVal - Front.mmVal; // how far above the back is the front?
float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front
// must repel, Back must attract
K_MAP bConsts = {FConsts.attracting, FConsts.repelling};
FBDiffErr.eDiff = eCurr - FBDiffErr.e;
if (!oor) {
FBDiffErr.eInt += eCurr;
FBDiffErr.eInt =
constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
}
FBDiffErr.e = eCurr;
FDiffPWM = pwmFunc(FConsts, FBDiffErr);
BDiffPWM = -pwmFunc(bConsts, FBDiffErr);
}
int16_t FullController::pwmFunc(K_MAP consts, Errors errs) {
if (oor)
return 0;
Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling;
return (int)(constants.kp * errs.e + constants.ki * errs.eInt +
constants.kd * errs.eDiff);
} }
// ── Serial report ────────────────────────────────────────────
void FullController::report() { void FullController::report() {
// CSV Format: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn // CSV: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
Serial.print(Left.mmVal); Serial.print(Left.mmVal); Serial.print(',');
Serial.print(","); Serial.print(Right.mmVal); Serial.print(',');
Serial.print(Right.mmVal); Serial.print(Front.mmVal); Serial.print(',');
Serial.print(","); Serial.print(Back.mmVal); Serial.print(',');
Serial.print(Front.mmVal); Serial.print(avg); Serial.print(',');
Serial.print(","); Serial.print(FLPWM); Serial.print(',');
Serial.print(Back.mmVal); Serial.print(BLPWM); Serial.print(',');
Serial.print(","); Serial.print(FRPWM); Serial.print(',');
Serial.print(avg); Serial.print(BRPWM); Serial.print(',');
Serial.print(",");
Serial.print(FLPWM);
Serial.print(",");
Serial.print(BLPWM);
Serial.print(",");
Serial.print(FRPWM);
Serial.print(",");
Serial.print(BRPWM);
Serial.print(",");
Serial.println(outputOn); Serial.println(outputOn);
} }
void FullController::updateAvgPID(Constants repel, Constants attract) { // ── Runtime tuning methods ───────────────────────────────────
avgConsts.repelling = repel; void FullController::updateHeightPID(PIDGains gains) { heightGains = gains; }
avgConsts.attracting = attract; void FullController::updateRollPID(PIDGains gains) { rollGains = gains; }
} void FullController::updatePitchPID(PIDGains gains) { pitchGains = gains; }
void FullController::updateLRPID(Constants down, Constants up) { void FullController::updateReference(float avgReference) {
LConsts.repelling = down;
LConsts.attracting = up;
}
void FullController::updateFBPID(Constants down, Constants up) {
FConsts.repelling = down;
FConsts.attracting = up;
}
void FullController::updateReferences(float avgReference, float lrDiffReference, float fbDiffReference) {
AvgRef = avgReference; AvgRef = avgReference;
LRDiffRef = lrDiffReference; }
FBDiffRef = fbDiffReference;
void FullController::setFeedforward(bool enabled) {
ffEnabled = enabled;
} }

115
lib/Controller.hpp
View File

@@ -2,103 +2,98 @@
#define CONTROLLER_HPP #define CONTROLLER_HPP
#include <stdint.h> #include <stdint.h>
#include <avr/pgmspace.h>
#include "IndSensorMap.hpp" #include "IndSensorMap.hpp"
// PIN MAPPING // ── Pin Mapping ──────────────────────────────────────────────
#define dirFR 2 #define dirBL 2
#define pwmFR 3 #define pwmBL 3
#define dirBR 4 #define dirBR 4
#define pwmBR 10 #define pwmBR 10
#define pwmFL 11 #define pwmFL 11
#define dirFL 7 #define dirFL 7
#define dirBL 8 #define dirFR 8
#define pwmBL 9 #define pwmFR 9
#define CAP 250 // ── Output Cap ───────────────────────────────────────────────
#define CAP 250 // Max PWM magnitude (0-255 Arduino range)
typedef struct Constants { // ── PID Gains (single set per loop — matches simulation) ────
typedef struct PIDGains {
float kp; float kp;
float ki; float ki;
float kd; float kd;
} Constants; } PIDGains;
typedef struct K_MAP { // ── PID Error State ──────────────────────────────────────────
Constants repelling; typedef struct PIDState {
Constants attracting; float e; // Current error
} K_MAP; float eDiff; // Derivative of error (e[k] - e[k-1])
float eInt; // Integral of error (accumulated)
} PIDState;
typedef struct FullConsts { // ── Feedforward LUT (PROGMEM) ────────────────────────────────
K_MAP avg; // Generated by gen_ff_lut.py from MaglevPredictor model
K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives. // Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
K_MAP fColl; // Positive = repelling, Negative = attracting
} FullConsts; // Beyond ~16mm: clamped to -CAP (no equilibrium exists)
#define FF_LUT_SIZE 64
#define FF_GAP_MIN 3.0f
#define FF_GAP_MAX 20.0f
#define FF_GAP_STEP 0.269841f
typedef struct Errors { // ── Geometry (mm, matching simulation) ───────────────────────
float e; // Sensor-to-sensor distances for angle computation
float eDiff; #define Y_DISTANCE_MM 101.6f // Left↔Right sensor spacing (mm)
float eInt; #define X_DISTANCE_MM 251.8f // Front↔Back sensor spacing (mm)
} Errors;
// ── Controller Class ─────────────────────────────────────────
class FullController { class FullController {
public: public:
bool oor; bool oor; // Any sensor out-of-range
bool outputOn; bool outputOn; // Enable/disable output
FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef) PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
: Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef), float avgRef, bool useFeedforward);
FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl),
FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}),
FBDiffErr({0,0,0}), oor(false), outputOn(false) {}
void update(); void update();
void zeroPWMs(); void zeroPWMs();
void sendOutputs(); void sendOutputs();
void report(); void report();
// PID tuning methods // Runtime tuning
void updateAvgPID(Constants repel, Constants attract); void updateHeightPID(PIDGains gains);
void updateLRPID(Constants down, Constants up); void updateRollPID(PIDGains gains);
void updateFBPID(Constants down, Constants up); void updatePitchPID(PIDGains gains);
void updateReference(float avgReference);
// Reference update methods void setFeedforward(bool enabled);
void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference);
private: private:
void avgControl(); int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
void LRControl(); int16_t feedforward(float gapMM);
void FBControl();
int16_t pwmFunc(K_MAP consts, Errors errs);
IndSensor& Left;
IndSensor& Right;
IndSensor& Front; IndSensor& Front;
IndSensor& Back; IndSensor& Back;
IndSensor& Right;
IndSensor& Left;
K_MAP avgConsts; PIDGains heightGains;
K_MAP LConsts; PIDGains rollGains;
K_MAP FConsts; PIDGains pitchGains;
Errors avgError; PIDState heightErr;
Errors LRDiffErr; PIDState rollErr;
Errors FBDiffErr; PIDState pitchErr;
float AvgRef; float AvgRef; // Target gap height (mm)
float LRDiffRef; float avg; // Current average gap (mm)
float FBDiffRef; bool ffEnabled; // Feedforward on/off
float avg;
int16_t avgPWM;
int16_t LDiffPWM;
int16_t RDiffPWM;
int16_t FDiffPWM;
int16_t BDiffPWM;
// Initially, I was going to make the Right and Back just the negatives,
// but having separate control functions running for each of these outputs might prove useful.
int16_t FLPWM; int16_t FLPWM;
int16_t BLPWM; int16_t BLPWM;
int16_t FRPWM; int16_t FRPWM;
int16_t BRPWM; int16_t BRPWM;
}; };
#endif // CONTROLLER_HPP #endif // CONTROLLER_HPP

8
lib/IndSensorMap.cpp
View File

@@ -58,7 +58,7 @@ float IndSensor::readMM() {
} }
// Predefined sensor instances // Predefined sensor instances
IndSensor indL(ind1Map, A0); IndSensor indF(ind1Map, A0);
IndSensor indR(ind0Map, A1); IndSensor indB(ind0Map, A1);
IndSensor indF(ind3Map, A5); IndSensor indR(ind3Map, A5);
IndSensor indB(ind2Map, A4); IndSensor indL(ind2Map, A4);