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 "FastPWM.hpp"
// K, Ki, Kd Constants
Constants repelling = {250, 0, 1000};
Constants attracting = {250, 0, 1000};
// ── PID Gains (Kp, Ki, Kd) ──────────────────────────────────
// Height loop: controls average gap → additive PWM on all coils
PIDGains heightGains = { 100.0f, 0.0f, 8.0f };
Constants RollLeftUp = {0, 0, 100};
Constants RollLeftDown = {0, 0, 100};
// Roll loop: corrects left/right tilt → differential L/R
PIDGains rollGains = { 0.6f, 0.0f, -0.1f };
Constants RollFrontUp = {0, 0, 500};
Constants RollFrontDown = {0, 0, 500};
// Pitch loop: corrects front/back tilt → differential F/B
PIDGains pitchGains = { 50.0f, 0.0f, 1.9f };
// Reference values for average dist,
float avgRef = 11.0; // TBD: what is our equilibrium height with this testrig?
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.
// ── Reference ────────────────────────────────────────────────
float avgRef = 12.36f; // Target gap height (mm) — 9.4 kg equilibrium
// Might be useful for things like jitter or lag.
#define sampling_rate 1000 // Hz
// ── Feedforward ──────────────────────────────────────────────
bool useFeedforward = true; // Set false to disable feedforward LUT
// EMA filter alpha value (all sensors use same alpha)
#define alphaVal 0.3f
// ── Sampling ─────────────────────────────────────────────────
#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.
// ═══════════════════════════════════════════════════════════════
unsigned long tprior;
unsigned int tDiffMicros;
FullConsts fullConsts = {
{repelling, attracting},
{RollLeftDown, RollLeftUp},
{RollFrontDown, RollFrontUp}
};
FullController controller(indL, indR, indF, indB,
heightGains, rollGains, pitchGains,
avgRef, useFeedforward);
FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef);
const int dt_micros = 1e6/sampling_rate;
#define LEV_ON
const int dt_micros = 1000000 / SAMPLING_RATE;
int ON = 0;
@@ -49,10 +46,10 @@ void setup() {
setupADC();
setupFastPWM();
indL.alpha = alphaVal;
indR.alpha = alphaVal;
indF.alpha = alphaVal;
indB.alpha = alphaVal;
indL.alpha = ALPHA_VAL;
indR.alpha = ALPHA_VAL;
indF.alpha = ALPHA_VAL;
indB.alpha = ALPHA_VAL;
tprior = micros();
@@ -71,91 +68,61 @@ void loop() {
String cmd = Serial.readStringUntil('\n');
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,")) {
int firstComma = cmd.indexOf(',');
int secondComma = cmd.indexOf(',', firstComma + 1);
int thirdComma = cmd.indexOf(',', secondComma + 1);
if (firstComma > 0 && secondComma > 0 && thirdComma > 0) {
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);
float newRef = cmd.substring(4).toFloat();
avgRef = newRef;
controller.updateReference(avgRef);
Serial.print("Updated Ref: ");
Serial.println(avgRef);
}
}
// Check if it's a PID tuning command (format: PID,mode,kp,ki,kd)
// PID,loop,kp,ki,kd — update gains (loop: 0=height, 1=roll, 2=pitch)
else if (cmd.startsWith("PID,")) {
int firstComma = cmd.indexOf(',');
int secondComma = cmd.indexOf(',', firstComma + 1);
int thirdComma = cmd.indexOf(',', secondComma + 1);
int fourthComma = cmd.indexOf(',', thirdComma + 1);
int c1 = cmd.indexOf(',');
int c2 = cmd.indexOf(',', c1 + 1);
int c3 = cmd.indexOf(',', c2 + 1);
int c4 = cmd.indexOf(',', c3 + 1);
if (firstComma > 0 && secondComma > 0 && thirdComma > 0 && fourthComma > 0) {
int mode = cmd.substring(firstComma + 1, secondComma).toInt();
float kp = cmd.substring(secondComma + 1, thirdComma).toFloat();
float ki = cmd.substring(thirdComma + 1, fourthComma).toFloat();
float kd = cmd.substring(fourthComma + 1).toFloat();
if (c1 > 0 && c2 > 0 && c3 > 0 && c4 > 0) {
int loop = cmd.substring(c1 + 1, c2).toInt();
float kp = cmd.substring(c2 + 1, c3).toFloat();
float ki = cmd.substring(c3 + 1, c4).toFloat();
float kd = cmd.substring(c4 + 1).toFloat();
Constants newConst = {kp, ki, kd};
PIDGains g = { kp, ki, kd };
// Mode mapping:
// 0: Repelling
// 1: Attracting
// 2: RollLeftDown
// 3: RollLeftUp
// 4: RollFrontDown
// 5: RollFrontUp
switch(mode) {
case 0: // Repelling
repelling = newConst;
controller.updateAvgPID(repelling, attracting);
Serial.println("Updated Repelling PID");
switch (loop) {
case 0:
heightGains = g;
controller.updateHeightPID(g);
Serial.println("Updated Height PID");
break;
case 1: // Attracting
attracting = newConst;
controller.updateAvgPID(repelling, attracting);
Serial.println("Updated Attracting PID");
case 1:
rollGains = g;
controller.updateRollPID(g);
Serial.println("Updated Roll PID");
break;
case 2: // RollLeftDown
RollLeftDown = newConst;
controller.updateLRPID(RollLeftDown, RollLeftUp);
Serial.println("Updated RollLeftDown 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");
case 2:
pitchGains = g;
controller.updatePitchPID(g);
Serial.println("Updated Pitch PID");
break;
default:
Serial.println("Invalid mode");
Serial.println("Invalid loop (0=height, 1=roll, 2=pitch)");
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');
}
}
@@ -166,11 +133,7 @@ void loop() {
controller.update();
controller.report();
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?
// since the floating point arithmetic may take a while...
tprior = micros();
}
//Serial.println(telapsed);
}

28
RL Testing/Function Fitting.ipynb
View File

@@ -2189,14 +2189,14 @@
"id": "21babeb3",
"metadata": {},
"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",
"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",
"execution_count": 50,
"execution_count": 1,
"id": "badbc379",
"metadata": {},
"outputs": [
@@ -2204,23 +2204,27 @@
"name": "stdout",
"output_type": "stream",
"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",
"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\n",
"======================================================================\n",
"Pod mass: 5.8 kg\n",
"Total weight: 56.898 N\n",
"Target force per yoke: 28.449 N\n",
"Pod mass: 9.4 kg\n",
"Total weight: 92.214 N\n",
"Target force per yoke: 46.107 N\n",
"Parameters: currL = 0 A, currR = 0 A, roll = 0°\n",
"\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"
]
}
],
"source": [
"# 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",
"from maglev_predictor import MaglevPredictor\n",
"from scipy.optimize import fsolve\n",
@@ -2228,15 +2232,15 @@
"# Initialize predictor\n",
"predictor = MaglevPredictor()\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",
"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",
"print(\"=\" * 70)\n",
"print(\"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\")\n",
"print(\"=\" * 70)\n",
"print(f\"Pod mass: 5.8 kg\")\n",
"print(f\"Total weight: {5.8 * 9.81:.3f} N\")\n",
"print(f\"Pod mass: 9.4 kg\")\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\"Parameters: currL = 0 A, currR = 0 A, roll = 0°\")\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,
mass=5.8,
mass=9.4,
I_roll=0.0192942414, # Ixx 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

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 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):
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()
# --- 8. Calculate Reward ---
# Goal: Hover at target gap (16.5mm), minimize roll/pitch, minimize power
target_gap = TARGET_GAP # 16.5mm in meters
# Goal: Hover at target gap (11.8mm), minimize roll/pitch, minimize power
target_gap = TARGET_GAP # 11.8mm in meters
avg_gap = (avg_gap_front + avg_gap_back) / 2
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()}
time = t['time']
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.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_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_TIMEOUT_S = 3600
TARGET_GAP_MM = TARGET_GAP * 1000

16
RL Testing/pid_best_params.json
View File

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

18
RL Testing/pid_best_params_1500.json
View File

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

18
RL Testing/pid_best_params_3000.json
View File

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

18
RL Testing/pid_best_params_6000.json
View File

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

2
RL Testing/pid_simulation.py
View File

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

12
RL Testing/pod.xml
View File

@@ -2,8 +2,8 @@
<robot name="lev_pod">
<link name="base_link">
<inertial>
<mass value="5.8"/>
<inertia ixx="0.0192942414" ixy="0.0" ixz="0.0" iyy="0.130582305" iyz="0.0" izz="0.13760599326"/>
<mass value="9.4"/>
<inertia ixx="0.03162537654" ixy="0.0" ixz="0.0" iyy="0.21929017831" iyz="0.0" izz="0.21430205089"/>
</inertial>
<collision>
@@ -13,19 +13,19 @@
Bolts
<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>
</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>
</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>
</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>
</collision>

18
RL Testing/temp.json
View File

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

265
lib/Controller.cpp
View File

@@ -1,32 +1,132 @@
#include "Controller.hpp"
#include <Arduino.h>
#include <math.h>
// CONTROLLER CONSTANTS
float MAX_INTEGRAL_TERM = 1e4;
// ── Integral windup limit ────────────────────────────────────
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() {
// 1. Read all sensors (updates mmVal, oor)
Left.readMM();
Right.readMM();
Front.readMM();
Back.readMM(); // read and update dists/oor for all sensors.
Back.readMM();
oor = Left.oor || Right.oor || Front.oor || Back.oor;
avgControl();
LRControl(); // run pwm functions.
FBControl();
// 2. Compute average gap (mm)
avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
FLPWM = constrain(avgPWM + LDiffPWM + FDiffPWM, -CAP, CAP);
BLPWM = constrain(avgPWM + LDiffPWM + BDiffPWM, -CAP, CAP);
FRPWM = constrain(avgPWM + RDiffPWM + FDiffPWM, -CAP, CAP);
BRPWM = constrain(avgPWM + RDiffPWM + BDiffPWM, -CAP, CAP);
// 3. Feedforward: base PWM from equilibrium LUT
int16_t ffPWM = ffEnabled ? feedforward(avg) : 0;
// FLPWM = avgPWM;
// BLPWM = avgPWM;
// FRPWM = avgPWM;
// BRPWM = avgPWM;
// 4. Height PID: error = reference - average gap
float heightE = AvgRef - avg;
heightErr.eDiff = heightE - heightErr.e;
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() {
FLPWM = 0;
BLPWM = 0;
@@ -34,131 +134,48 @@ void FullController::zeroPWMs() {
BRPWM = 0;
}
// ── Send PWM values to hardware ──────────────────────────────
void FullController::sendOutputs() {
if (!outputOn) {
zeroPWMs();
}
// The following assumes 0 direction drives repulsion and 1 direction drives
// attraction. Using direct register writes to maintain fast PWM mode set by
// setupFastPWM()
// Direction: LOW = repelling (positive PWM), HIGH = attracting (negative PWM)
// Using direct register writes for fast PWM mode set by setupFastPWM()
digitalWrite(dirFL, FLPWM < 0);
OCR2A = abs(FLPWM); // Pin 11 -> Timer 2A
OCR2A = abs(FLPWM); // Pin 11 → Timer 2A
digitalWrite(dirBL, BLPWM < 0);
OCR1A = abs(BLPWM); // Pin 9 -> Timer 1A
OCR1A = abs(BLPWM); // Pin 9 → Timer 1A
digitalWrite(dirFR, FRPWM < 0);
OCR2B = abs(FRPWM); // Pin 3 -> Timer 2B
OCR2B = abs(FRPWM); // Pin 3 → Timer 2B
digitalWrite(dirBR, BRPWM < 0);
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);
OCR1B = abs(BRPWM); // Pin 10 → Timer 1B
}
// ── Serial report ────────────────────────────────────────────
void FullController::report() {
// CSV Format: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
Serial.print(Left.mmVal);
Serial.print(",");
Serial.print(Right.mmVal);
Serial.print(",");
Serial.print(Front.mmVal);
Serial.print(",");
Serial.print(Back.mmVal);
Serial.print(",");
Serial.print(avg);
Serial.print(",");
Serial.print(FLPWM);
Serial.print(",");
Serial.print(BLPWM);
Serial.print(",");
Serial.print(FRPWM);
Serial.print(",");
Serial.print(BRPWM);
Serial.print(",");
// CSV: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
Serial.print(Left.mmVal); Serial.print(',');
Serial.print(Right.mmVal); Serial.print(',');
Serial.print(Front.mmVal); Serial.print(',');
Serial.print(Back.mmVal); Serial.print(',');
Serial.print(avg); 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);
}
void FullController::updateAvgPID(Constants repel, Constants attract) {
avgConsts.repelling = repel;
avgConsts.attracting = attract;
}
// ── Runtime tuning methods ───────────────────────────────────
void FullController::updateHeightPID(PIDGains gains) { heightGains = gains; }
void FullController::updateRollPID(PIDGains gains) { rollGains = gains; }
void FullController::updatePitchPID(PIDGains gains) { pitchGains = gains; }
void FullController::updateLRPID(Constants down, Constants up) {
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) {
void FullController::updateReference(float 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
#include <stdint.h>
#include <avr/pgmspace.h>
#include "IndSensorMap.hpp"
// PIN MAPPING
#define dirFR 2
#define pwmFR 3
// ── Pin Mapping ──────────────────────────────────────────────
#define dirBL 2
#define pwmBL 3
#define dirBR 4
#define pwmBR 10
#define pwmFL 11
#define dirFL 7
#define dirBL 8
#define pwmBL 9
#define dirFR 8
#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 ki;
float kd;
} Constants;
} PIDGains;
typedef struct K_MAP {
Constants repelling;
Constants attracting;
} K_MAP;
// ── PID Error State ──────────────────────────────────────────
typedef struct PIDState {
float e; // Current error
float eDiff; // Derivative of error (e[k] - e[k-1])
float eInt; // Integral of error (accumulated)
} PIDState;
typedef struct FullConsts {
K_MAP avg;
K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives.
K_MAP fColl;
} FullConsts;
// ── Feedforward LUT (PROGMEM) ────────────────────────────────
// Generated by gen_ff_lut.py from MaglevPredictor model
// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
// Positive = repelling, Negative = attracting
// 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 {
float e;
float eDiff;
float eInt;
} Errors;
// ── Geometry (mm, matching simulation) ───────────────────────
// Sensor-to-sensor distances for angle computation
#define Y_DISTANCE_MM 101.6f // Left↔Right sensor spacing (mm)
#define X_DISTANCE_MM 251.8f // Front↔Back sensor spacing (mm)
// ── Controller Class ─────────────────────────────────────────
class FullController {
public:
bool oor;
bool outputOn;
bool oor; // Any sensor out-of-range
bool outputOn; // Enable/disable output
FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef)
: Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef),
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) {}
PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
float avgRef, bool useFeedforward);
void update();
void zeroPWMs();
void sendOutputs();
void report();
// PID tuning methods
void updateAvgPID(Constants repel, Constants attract);
void updateLRPID(Constants down, Constants up);
void updateFBPID(Constants down, Constants up);
// Reference update methods
void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference);
// Runtime tuning
void updateHeightPID(PIDGains gains);
void updateRollPID(PIDGains gains);
void updatePitchPID(PIDGains gains);
void updateReference(float avgReference);
void setFeedforward(bool enabled);
private:
void avgControl();
void LRControl();
void FBControl();
int16_t pwmFunc(K_MAP consts, Errors errs);
int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
int16_t feedforward(float gapMM);
IndSensor& Left;
IndSensor& Right;
IndSensor& Front;
IndSensor& Back;
IndSensor& Right;
IndSensor& Left;
K_MAP avgConsts;
K_MAP LConsts;
K_MAP FConsts;
PIDGains heightGains;
PIDGains rollGains;
PIDGains pitchGains;
Errors avgError;
Errors LRDiffErr;
Errors FBDiffErr;
PIDState heightErr;
PIDState rollErr;
PIDState pitchErr;
float AvgRef;
float LRDiffRef;
float FBDiffRef;
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.
float AvgRef; // Target gap height (mm)
float avg; // Current average gap (mm)
bool ffEnabled; // Feedforward on/off
int16_t FLPWM;
int16_t BLPWM;
int16_t FRPWM;
int16_t BRPWM;
};
#endif // CONTROLLER_HPP

8
lib/IndSensorMap.cpp
View File

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