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

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

141
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;
public:
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) {}
FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
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);
void update();
void zeroPWMs();
void sendOutputs();
void report();
private:
void avgControl();
void LRControl();
void FBControl();
int16_t pwmFunc(K_MAP consts, Errors errs);
// Runtime tuning
void updateHeightPID(PIDGains gains);
void updateRollPID(PIDGains gains);
void updatePitchPID(PIDGains gains);
void updateReference(float avgReference);
void setFeedforward(bool enabled);
IndSensor& Front;
IndSensor& Back;
IndSensor& Right;
IndSensor& Left;
private:
int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
int16_t feedforward(float gapMM);
K_MAP avgConsts;
K_MAP LConsts;
K_MAP FConsts;
IndSensor& Left;
IndSensor& Right;
IndSensor& Front;
IndSensor& Back;
Errors avgError;
Errors LRDiffErr;
Errors FBDiffErr;
PIDGains heightGains;
PIDGains rollGains;
PIDGains pitchGains;
float AvgRef;
float LRDiffRef;
float FBDiffRef;
float avg;
PIDState heightErr;
PIDState rollErr;
PIDState pitchErr;
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;
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);