From d7da1172e16a7da3253b6d98d1cb354820f97fa5 Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Sun, 16 Nov 2025 20:05:18 -0600 Subject: [PATCH] redesign --- Controller.cpp | 137 ++++++++++++ Controller.hpp | 105 +++++++++ DualYokeControl (Previous Version).ino | 200 ------------------ IndSensorMap.cpp | 5 +- IndSensorMap.hpp | 26 +-- ...IndivCoilControl (AI Slop Version).ino.bak | 4 +- lev_control_4pt_small.ino | 84 ++++++++ 7 files changed, 345 insertions(+), 216 deletions(-) create mode 100644 Controller.cpp create mode 100644 Controller.hpp delete mode 100644 DualYokeControl (Previous Version).ino rename IndivCoilControl (AI Slop Version).ino => IndivCoilControl (AI Slop Version).ino.bak (96%) create mode 100644 lev_control_4pt_small.ino diff --git a/Controller.cpp b/Controller.cpp new file mode 100644 index 0000000..7f42115 --- /dev/null +++ b/Controller.cpp @@ -0,0 +1,137 @@ +#include "Controller.hpp" +#include + +// CONTROLLER CONSTANTS +float MAX_INTEGRAL_TERM = 1e4; + +void FullController::update(float tDiff) { + this->tDiff = tDiff; // store time step for use in differential and integral portions + + Left.readMM(); + Right.readMM(); + Front.readMM(); + Back.readMM(); // read and update dists/oor for all sensors. + + oor = Left.oor || Right.oor || Front.oor || Back.oor; + + avgControl(); + LRControl(); // run pwm functions. + FBControl(); + + int16_t flTarget = avgPWM + LDiffPWM + FDiffPWM; + int16_t blTarget = avgPWM + LDiffPWM + BDiffPWM; + int16_t frTarget = avgPWM + RDiffPWM + FDiffPWM; + int16_t brTarget = avgPWM + RDiffPWM + BDiffPWM; + + FLPWM = slewLimit(flTarget, FLPrev); + BLPWM = slewLimit(blTarget, BLPrev); + FRPWM = slewLimit(frTarget, FRPrev); + BRPWM = slewLimit(brTarget, BRPrev); + + FLPrev = FLPWM; + BLPrev = BLPWM; + FRPrev = FRPWM; + BRPrev = BRPWM; +} + +void FullController::zeroPWMs() { + FLPWM = 0; + BLPWM = 0; + FRPWM = 0; + BRPWM = 0; +} + +void FullController::sendOutputs() { + if (!outputOn) { + zeroPWMs(); + } + + // The following assumes 0 direction drives repulsion and 1 direction drives attraction. + digitalWrite(dirFL, FLPWM < 0); + analogWrite(pwmFL, abs(FLPWM)); + digitalWrite(dirBL, BLPWM < 0); + analogWrite(pwmBL, abs(BLPWM)); + digitalWrite(dirFR, FRPWM < 0); + analogWrite(pwmFR, abs(FRPWM)); + digitalWrite(dirBR, BRPWM < 0); + analogWrite(pwmBR, abs(BRPWM)); +} + +void FullController::avgControl() { + float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) / 4.0; + float eCurr = AvgRef - avg; // assuming up is positive and down is negative + + avgError.eDiff = (tDiff == 0) ? 0:(eCurr - avgError.e) / tDiff; // rise over run + avgError.eInt += eCurr * tDiff; + 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 = (tDiff == 0) ? 0:(eCurr - LRDiffErr.e) / tDiff; + LRDiffErr.eInt += eCurr * tDiff; + 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 = (tDiff == 0) ? 0:(eCurr - FBDiffErr.e) / tDiff; + FBDiffErr.eInt += eCurr * tDiff; + 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)constrain(constants.K*(errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff), -(float)CAP,(float)CAP); +} + +int16_t FullController::slewLimit(int16_t target, int16_t prev) { + int16_t maxChange = (int16_t)(slewRateLimit * tDiff); + int16_t delta = target - prev; + if (abs(delta) <= maxChange) return target; + return prev + (delta > 0 ? maxChange : -maxChange); +} + +void FullController::report() { + Serial.print("SENSORS - Left: "); + Serial.print(Left.mmVal); + Serial.print("mm, Right: "); + Serial.print(Right.mmVal); + Serial.print("mm, Front: "); + Serial.print(Front.mmVal); + Serial.print("mm, Back: "); + Serial.print(Back.mmVal); + Serial.print("mm,\n"); + + Serial.print("PWMS - FL_PWM: "); + Serial.print(FLPWM); + Serial.print(", BL_PWM: "); + Serial.print(BLPWM); + Serial.print("FR_PWM: "); + Serial.print(FRPWM); + Serial.print("BR_PWM: "); + Serial.print(BRPWM); + Serial.print("\n"); + + Serial.print("CONTROL ON - "); + Serial.print(outputOn); + Serial.print("\n"); +} \ No newline at end of file diff --git a/Controller.hpp b/Controller.hpp new file mode 100644 index 0000000..2988335 --- /dev/null +++ b/Controller.hpp @@ -0,0 +1,105 @@ +#ifndef CONTROLLER_HPP +#define CONTROLLER_HPP + +#include +#include "IndSensorMap.hpp" + +// PIN MAPPING +#define dirFR 2 +#define pwmFR 3 +#define dirBR 4 +#define pwmBR 5 +#define pwmFL 6 +#define dirFL 7 +#define dirBL 8 +#define pwmBL 9 + +#define CAP 200 + +typedef struct Constants { + float K; + float ki; + float kd; +} Constants; + +typedef struct K_MAP { + Constants repelling; + Constants attracting; +} K_MAP; + +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; + +typedef struct Errors { + float e; + float eDiff; + float eInt; +} Errors; + +class FullController { + public: + bool oor; + bool outputOn; + + FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, + FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef, float slewRate) + : 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), + FLPrev(0), BLPrev(0), FRPrev(0), BRPrev(0), slewRateLimit(slewRate) {} + + void update(float tDiff); + void zeroPWMs(); + void sendOutputs(); + void report(); + + private: + void avgControl(); + void LRControl(); + void FBControl(); + int16_t pwmFunc(K_MAP consts, Errors errs); + int16_t slewLimit(int16_t target, int16_t prev); + + IndSensor& Front; + IndSensor& Back; + IndSensor& Right; + IndSensor& Left; + + K_MAP avgConsts; + K_MAP LConsts; + K_MAP FConsts; + + Errors avgError; + Errors LRDiffErr; + Errors FBDiffErr; + + float AvgRef; + float LRDiffRef; + float FBDiffRef; + float slewRateLimit; + + 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 BLPWM; + int16_t FRPWM; + int16_t BRPWM; + + int16_t FLPrev; + int16_t BLPrev; + int16_t FRPrev; + int16_t BRPrev; + + unsigned int tDiff; +}; +#endif // CONTROLLER_HPP \ No newline at end of file diff --git a/DualYokeControl (Previous Version).ino b/DualYokeControl (Previous Version).ino deleted file mode 100644 index 5827d53..0000000 --- a/DualYokeControl (Previous Version).ino +++ /dev/null @@ -1,200 +0,0 @@ -#include -#include "IndSensorMap.hpp" - -// PIN MAPPING - -#define dirFR 2 -#define pwmFR 3 -#define dirBR 4 -#define pwmBR 5 -#define pwmFL 6 -#define dirFL 7 -#define dirBL 8 -#define pwmBL 9 - -// variables - -int dist_raw, tprior, telapsed, pwm, pwm2, dist2_raw; -bool oor, oor2; -float dist,ecurr, eprior, derror, ecum, ff,dist2,ecurr2, eprior2, derror2, ecum2, ff2; - -#define CAP 200 - - -// CONTROLLER CONSTANTS -float MAX_INTEGRAL_TERM = 1e4; - -//// FOR MC 1 -//const float K_f = 50; // gain for when we want to fall (ref > dist or error > 0) -//const float ki_f = 0.01; -//const float kd_f = 8; // 25 -// -//const float K_a = 20; -//const float ki_a = ki_f; -//const float kd_a = 10; //30; - -typedef struct Constants { - float K; - float ki; - float kd; -} Constants; - -typedef struct K_MAP { - Constants falling; - Constants attracting; -} K_MAP; - -typedef struct Collective { - K_MAP constants; - float e; - float eDiff; - float eInt; - float ref; -} Collective; - -Collective collLeft = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 21.0}; -Collective collRight = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 22.0}; - -int levCollective(Collective collective, bool oor){ - if (oor){ - pwm = 0; - } - else{ - Constants pidConsts; - - // this means that dist > ref so we gotta attract to track now vv - if (collective.e < 0) pidConsts = collective.constants.attracting; - // this is falling vv - else pidConsts = collective.constants.falling; - - pwm = constrain(pidConsts.K*(collective.e + pidConsts.ki*collective.eInt + pidConsts.kd*collective.eDiff), -CAP,CAP); - } - return (int)pwm; -} - -#define sampling_rate 1000 -const int dt_micros = 1e6/sampling_rate; - -#define LEV_ON - -int ON = 0; - -void setup() { - // put your setup code here, to run once: - - Serial.begin(57600); - - tprior = micros(); - ecum = 0; - ecum2 = 0; - - // positive pwm is A - // negative is B - - // ATTRACT IS B // REPEL IS A - - //when error is negative, I want to attract. - send_pwmFL(0); - send_pwmFR(0); - -} - -void loop() { - if (Serial.available() > 0) { - String inputString = Serial.readStringUntil('\n'); // Read the full input - inputString.trim(); // Remove leading/trailing whitespace, including \n and \r - - // Conditional pipeline - if (inputString == "0") { - ON=0; - } - else { - ON=1; - } - } - - telapsed = micros() - tprior; - - if (telapsed >= dt_micros){ - // put your main code here, to run repeatedly: - indL.read(); - indR.read(); - indF.read(); - indB.read(); - dist_raw = analogRead(indL); - if (dist_raw > 870) oor = true; - dist = indToMM(ind0Map, dist_raw); // 189->950, 16->26 - Serial.print(dist); - Serial.print(", "); - - dist2_raw = analogRead(indR); - if (dist2_raw > 870) oor2 = true; - dist2 = indToMM(ind1Map, dist2_raw); - Serial.print(dist2); - Serial.print(", "); - - ecurr = ref - dist; - derror = ecurr - eprior; - - ecurr2 = ref2 - dist2; - derror2 = ecurr2 - eprior2; - - ecum += ecurr * (telapsed / 1e6); - ecum = constrain(ecum, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); - ecum2 += ecurr2 * (telapsed / 1e6); - ecum2 = constrain(ecum2, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); - - - if (ON) { - int collective1 = - send_pwmFL(pwm); - send_pwmFR(pwm2); - Serial.print(pwm); - Serial.print(", "); - Serial.print(pwm2); - Serial.print(", "); - } - else { - send_pwmFL(0); - send_pwmFR(0); - Serial.print(0); - Serial.print(", "); - Serial.print(0); - Serial.print(", "); - } - - Serial.print(ON); - - tprior = micros(); - eprior = ecurr; - eprior2 = ecurr2; -// //Serial.print(ecurr); Serial.print(","); Serial.print(oor); Serial.print(","); Serial.print(derror); Serial.print(","); Serial.print(pwm); Serial.print("; "); Serial.print(ecurr2); Serial.print(","); Serial.print(oor2); Serial.print(","); Serial.print(derror2); Serial.print(","); Serial.print(pwm2); -// Serial.print(ecurr); Serial.print(","); Serial.print(ecurr2); Serial.print(","); Serial.print(ecum); Serial.print(",");Serial.print(ecum2); Serial.print(","); -// - Serial.println(); - } - - //Serial.println(telapsed); -} - -void send_pwmFL(int val){ - if (val > 0) { - digitalWrite(dirFL, LOW); - } - else{ - digitalWrite(dirFL,HIGH); - } - analogWrite(pwmFL,abs(val)); -} - -void send_pwmFR(int val){ - if (val > 0) { - digitalWrite(dirFR, LOW); - } - else{ - digitalWrite(dirFR,HIGH); - } - - analogWrite(pwmFR,abs(val)); - -} diff --git a/IndSensorMap.cpp b/IndSensorMap.cpp index c95108f..fdae3cd 100644 --- a/IndSensorMap.cpp +++ b/IndSensorMap.cpp @@ -18,10 +18,11 @@ float IndSensor::toMM(unsigned int raw) { } // Read sensor directly from pin and convert to millimeters -float IndSensor::read() { +float IndSensor::readMM() { unsigned int raw = analogRead(pin); oor = (raw == 0 || raw > 870); // Update out-of-range flag - return toMM(raw); + mmVal = toMM(raw); + return mmVal; } // Predefined sensor instances diff --git a/IndSensorMap.hpp b/IndSensorMap.hpp index 187dd03..24a8b9b 100644 --- a/IndSensorMap.hpp +++ b/IndSensorMap.hpp @@ -1,6 +1,8 @@ #ifndef IND_SENSOR_MAP_HPP #define IND_SENSOR_MAP_HPP +#include + // Inductive Sensor Mapping Struct typedef struct IndSensorMap { double A; @@ -11,21 +13,21 @@ typedef struct IndSensorMap { } IndSensorMap; class IndSensor { -public: - bool oor; - int mmVal; + public: + bool oor; + float mmVal; - // Constructor - IndSensor(IndSensorMap calibration, uint8_t analogPin); - // Read sensor directly from pin and convert to millimeters - float read(); + // Constructor + IndSensor(IndSensorMap calibration, uint8_t analogPin); + // Read sensor directly from pin and convert to millimeters + float readMM(); -private: - IndSensorMap consts; - uint8_t pin; + private: + IndSensorMap consts; + uint8_t pin; - // helper function to convert analog reading to millimeters - float toMM(unsigned int raw); + // helper function to convert analog reading to millimeters + float toMM(unsigned int raw); }; // sensor instances diff --git a/IndivCoilControl (AI Slop Version).ino b/IndivCoilControl (AI Slop Version).ino.bak similarity index 96% rename from IndivCoilControl (AI Slop Version).ino rename to IndivCoilControl (AI Slop Version).ino.bak index 6977159..f498382 100755 --- a/IndivCoilControl (AI Slop Version).ino +++ b/IndivCoilControl (AI Slop Version).ino.bak @@ -84,8 +84,8 @@ const float ref2 = 22.0; // right yoke distance setpoint const float K_current = 1; // (kept for compatibility) const float ki_current = 0; // (kept for compatibility) -// FOR MC 1 (left yoke): separate gains when falling vs attracting -const float K_f = 40; // falling (error > 0) — push down / reduce lift +// FOR MC 1 (left yoke): separate gains when repelling vs attracting +const float K_f = 40; // repelling (error > 0) — push down / reduce lift const float ki_f = 0.01; const float kd_f = 7; diff --git a/lev_control_4pt_small.ino b/lev_control_4pt_small.ino new file mode 100644 index 0000000..a5566e6 --- /dev/null +++ b/lev_control_4pt_small.ino @@ -0,0 +1,84 @@ +#include +#include "IndSensorMap.hpp" +#include "Controller.hpp" + +// K, Ki, Kd Constants +Constants repelling = {40, 0.01, 7}; +Constants attracting = {20, 0.01, 20}; + +Constants RollLeftUp = {30, 0.01, 15}; +Constants RollLeftDown = {30, 0.01, 15}; + +Constants RollFrontUp = {30, 0.01, 15}; +Constants RollFrontDown = {30, 0.01, 15}; + +// Reference values for average dist, +float avgRef = 21.0; // TBD: what is our equilibrium height with this testrig? +float LRDiffRef = 0.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. + +float slewRateLimit = 100.0; // max PWM change per control cycle (determined by 1 second / sampling rate) +// this was implemented by Claude and we can see if it helps. +// Set it at or above 255 to make it have no effect. + +// Might be useful for things like jitter or lag. +#define sampling_rate 1000 // Hz + +// 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, fullConsts, avgRef, LRDiffRef, FBDiffRef, slewRateLimit); + +const int dt_micros = 1e6/sampling_rate; + +#define LEV_ON + +int ON = 0; + +void setup() { + Serial.begin(57600); + + tprior = micros(); + + pinMode(dirFL, OUTPUT); + pinMode(pwmFL, OUTPUT); + pinMode(dirBL, OUTPUT); + pinMode(pwmBL, OUTPUT); + pinMode(dirFR, OUTPUT); + pinMode(pwmFR, OUTPUT); + pinMode(dirBR, OUTPUT); + pinMode(pwmBR, OUTPUT); +} + +void loop() { + if (Serial.available() > 0) { + String inputString = Serial.readStringUntil('\n'); // Read the full input + inputString.trim(); // Remove leading/trailing whitespace, including \n and \r + + // determine whether control is on or off based on serial input. + if (inputString == "0") controller.outputOn=0; + else controller.outputOn=1; + } + + tDiffMicros = micros() - tprior; + + if (tDiffMicros >= dt_micros){ + controller.update((float)tDiffMicros / (float)1e6); + 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... + } + + //Serial.println(telapsed); +} \ No newline at end of file