From 1e9fcfa1af5ac93e812c6d8127f14784f8d454ff Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Thu, 20 Nov 2025 15:13:19 -0600 Subject: [PATCH] added PseudoSensorControl plus lots of symlinks and organization --- .../AdditiveControlCode.ino | 0 .../Controller.cpp | 0 .../Controller.hpp | 0 .../IndSensorMap.cpp | 0 .../IndSensorMap.hpp | 0 AdditiveControlCode/PseudoSensorControl.cpp | 1 + AdditiveControlCode/PseudoSensorControl.hpp | 1 + DistanceReporting/DistanceReporting.ino | 6 -- .../IndSensorMap.cpp | 0 .../IndSensorMap.hpp | 0 PseudoSensorControl/PseudoSensorControl.cpp | 1 + PseudoSensorControl/PseudoSensorControl.hpp | 1 + PseudoSensorControl/PseudoSensorControl.ino | 62 ++++++++++++ lib/PseudoSensorControl.cpp | 95 +++++++++++++++++++ lib/PseudoSensorControl.hpp | 81 ++++++++++++++++ 15 files changed, 242 insertions(+), 6 deletions(-) rename PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino => AdditiveControlCode/AdditiveControlCode.ino (100%) rename {PIDTesting-2yoke4coil => AdditiveControlCode}/Controller.cpp (100%) rename {PIDTesting-2yoke4coil => AdditiveControlCode}/Controller.hpp (100%) rename {DistanceReporting => AdditiveControlCode}/IndSensorMap.cpp (100%) rename {DistanceReporting => AdditiveControlCode}/IndSensorMap.hpp (100%) create mode 120000 AdditiveControlCode/PseudoSensorControl.cpp create mode 120000 AdditiveControlCode/PseudoSensorControl.hpp delete mode 100644 DistanceReporting/DistanceReporting.ino rename {PIDTesting-2yoke4coil => PseudoSensorControl}/IndSensorMap.cpp (100%) rename {PIDTesting-2yoke4coil => PseudoSensorControl}/IndSensorMap.hpp (100%) create mode 120000 PseudoSensorControl/PseudoSensorControl.cpp create mode 120000 PseudoSensorControl/PseudoSensorControl.hpp create mode 100644 PseudoSensorControl/PseudoSensorControl.ino create mode 100644 lib/PseudoSensorControl.cpp create mode 100644 lib/PseudoSensorControl.hpp diff --git a/PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino b/AdditiveControlCode/AdditiveControlCode.ino similarity index 100% rename from PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino rename to AdditiveControlCode/AdditiveControlCode.ino diff --git a/PIDTesting-2yoke4coil/Controller.cpp b/AdditiveControlCode/Controller.cpp similarity index 100% rename from PIDTesting-2yoke4coil/Controller.cpp rename to AdditiveControlCode/Controller.cpp diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/AdditiveControlCode/Controller.hpp similarity index 100% rename from PIDTesting-2yoke4coil/Controller.hpp rename to AdditiveControlCode/Controller.hpp diff --git a/DistanceReporting/IndSensorMap.cpp b/AdditiveControlCode/IndSensorMap.cpp similarity index 100% rename from DistanceReporting/IndSensorMap.cpp rename to AdditiveControlCode/IndSensorMap.cpp diff --git a/DistanceReporting/IndSensorMap.hpp b/AdditiveControlCode/IndSensorMap.hpp similarity index 100% rename from DistanceReporting/IndSensorMap.hpp rename to AdditiveControlCode/IndSensorMap.hpp diff --git a/AdditiveControlCode/PseudoSensorControl.cpp b/AdditiveControlCode/PseudoSensorControl.cpp new file mode 120000 index 0000000..c0c326e --- /dev/null +++ b/AdditiveControlCode/PseudoSensorControl.cpp @@ -0,0 +1 @@ +../lib/PseudoSensorControl.cpp \ No newline at end of file diff --git a/AdditiveControlCode/PseudoSensorControl.hpp b/AdditiveControlCode/PseudoSensorControl.hpp new file mode 120000 index 0000000..14c2830 --- /dev/null +++ b/AdditiveControlCode/PseudoSensorControl.hpp @@ -0,0 +1 @@ +../lib/PseudoSensorControl.hpp \ No newline at end of file diff --git a/DistanceReporting/DistanceReporting.ino b/DistanceReporting/DistanceReporting.ino deleted file mode 100644 index a5dbdd7..0000000 --- a/DistanceReporting/DistanceReporting.ino +++ /dev/null @@ -1,6 +0,0 @@ -#include - -void setup() { - Serial.begin(57600); - -} \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.cpp b/PseudoSensorControl/IndSensorMap.cpp similarity index 100% rename from PIDTesting-2yoke4coil/IndSensorMap.cpp rename to PseudoSensorControl/IndSensorMap.cpp diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PseudoSensorControl/IndSensorMap.hpp similarity index 100% rename from PIDTesting-2yoke4coil/IndSensorMap.hpp rename to PseudoSensorControl/IndSensorMap.hpp diff --git a/PseudoSensorControl/PseudoSensorControl.cpp b/PseudoSensorControl/PseudoSensorControl.cpp new file mode 120000 index 0000000..c0c326e --- /dev/null +++ b/PseudoSensorControl/PseudoSensorControl.cpp @@ -0,0 +1 @@ +../lib/PseudoSensorControl.cpp \ No newline at end of file diff --git a/PseudoSensorControl/PseudoSensorControl.hpp b/PseudoSensorControl/PseudoSensorControl.hpp new file mode 120000 index 0000000..14c2830 --- /dev/null +++ b/PseudoSensorControl/PseudoSensorControl.hpp @@ -0,0 +1 @@ +../lib/PseudoSensorControl.hpp \ No newline at end of file diff --git a/PseudoSensorControl/PseudoSensorControl.ino b/PseudoSensorControl/PseudoSensorControl.ino new file mode 100644 index 0000000..50fd073 --- /dev/null +++ b/PseudoSensorControl/PseudoSensorControl.ino @@ -0,0 +1,62 @@ +#include +#include "IndSensorMap.hpp" +#include "PseudoSensorControl.hpp" + +float refs[4] = {10.83,10.83,10.83,10.83}; + +Constants repelling = {40, 0.01, 7}; +Constants attracting = {20, 0.01, 20}; + +K_MAP consts = {repelling, attracting}; + +#define slewRateLimit 100 // 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; + +PseudoSensorController controller(indL, indR, indF, indB, consts, refs, slewRateLimit); + +const int dt_micros = 1e6/sampling_rate; + +#define LEV_ON + +int ON = 0; + +void setup() { + Serial.begin(57600); + + tprior = micros(); + for (PinPair& mc : pinMap) { + pinMode(mc.dir, OUTPUT); + pinMode(mc.pwm, OUTPUT); + } +} + +void loop() { + if (Serial.available() > 0) { + // this might need to be changed if we have trouble getting serial to read. + char c = Serial.read(); + while(Serial.available()) Serial.read(); // flush remaining + + controller.outputOn = (c != '0'); + } + + tDiffMicros = micros() - tprior; + + if (tDiffMicros >= dt_micros){ + 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... + } +} \ No newline at end of file diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp new file mode 100644 index 0000000..b323bc0 --- /dev/null +++ b/lib/PseudoSensorControl.cpp @@ -0,0 +1,95 @@ +#include "PseudoSensorControl.hpp" +#include + +// CONTROLLER CONSTANTS +float MAX_INTEGRAL_TERM = 1e4; + +PinPair pinMap[4] = {{dirFL, pwmFL}, {dirFR, pwmFR}, {dirBL, pwmBL}, {dirBR, pwmBR}}; + +void PseudoSensorController::update() { + + 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; + + control(); + + for (uint8_t i = 0; i < 4; i++) { + PWMs[i] = slewLimit(PWMs[i], Prevs[i]); + Prevs[i] = PWMs[i]; + } +} + +void PseudoSensorController::zeroPWMs() { + memset(PWMs, 0, sizeof(PWMs)); +} + +void PseudoSensorController::sendOutputs() { + if (!outputOn) zeroPWMs(); + + for (uint8_t i = 0; i < 4; i++) { + // The following assumes 0 direction drives repulsion and 1 direction drives attraction. + digitalWrite(pinMap[i].dir, PWMs[i] < 0); + analogWrite(pinMap[i].pwm, abs(PWMs[i])); + } +} + +void PseudoSensorController::control() { + float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f; + float pseudos[4] = {Front.mmVal + Left.mmVal - avg, + Front.mmVal + Right.mmVal - avg, + Back.mmVal + Left.mmVal - avg, + Back.mmVal + Right.mmVal - avg}; + + for (uint8_t i = 0; i < 4; i++) { + float eCurr = Refs[i] - pseudos[i]; // Above reference is positive error. + + errors[i].eDiff = (eCurr - errors[i].e); // rise over run + errors[i].eInt += eCurr; + errors[i].eInt = constrain(errors[i].eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + errors[i].e = eCurr; + + PWMs[i] = pwmFunc(Consts, errors[i]); + } +} + +int16_t PseudoSensorController::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 PseudoSensorController::slewLimit(int16_t target, int16_t prev) { + int16_t delta = target - prev; + if (abs(delta) <= slewRateLimit) return target; + return prev + (delta > 0 ? slewRateLimit : -slewRateLimit); +} + +void PseudoSensorController::report() { + Serial.print("CONTROL ON - "); + Serial.print(outputOn); + Serial.print("\n"); + + 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(PWMs[0]); + Serial.print(", FR_PWM: "); + Serial.print(PWMs[1]); + Serial.print("BL_PWM: "); + Serial.print(PWMs[2]); + Serial.print("BR_PWM: "); + Serial.print(PWMs[3]); + Serial.print("\n"); +} \ No newline at end of file diff --git a/lib/PseudoSensorControl.hpp b/lib/PseudoSensorControl.hpp new file mode 100644 index 0000000..95ad9d2 --- /dev/null +++ b/lib/PseudoSensorControl.hpp @@ -0,0 +1,81 @@ +#ifndef PSEUDOSENSORCONTROLLER_HPP +#define PSEUDOSENSORCONTROLLER_HPP + +#include +#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 + +typedef struct PinPair { + const uint8_t dir; + const uint8_t pwm; +} PinPair; + +extern PinPair pinMap[4]; +// FL, FR, BL, BR + +#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 Errors { + float e; + float eDiff; + float eInt; +} Errors; + +class PseudoSensorController { + public: + bool oor; + bool outputOn; + + PseudoSensorController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, + K_MAP consts, float* refs, uint16_t slewRate) : Left(l), Right(r), Front(f), + Back(b), Refs(refs), errors{}, Consts(consts), oor(false), outputOn(false), + Prevs{}, slewRateLimit(slewRate) {} + + void update(); + void zeroPWMs(); + void sendOutputs(); + void report(); + + private: + void control(); + 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 Consts; + + Errors errors[4]; // FL FR BL BR + + float* Refs; // length 4 FL FR BL BR + uint16_t slewRateLimit; + + int16_t PWMs[4]; // FL FR BL BR + + int16_t Prevs[4]; // FL FR BL BR +}; +#endif // PSEUDOSENSORCONTROLLER_HPP \ No newline at end of file