Loaded new mass and inertial characteristics into sim, ported sim arch into arduino code
This commit is contained in:
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user