From 604009db9a20290810634238e1a4d7942afb09ff Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Wed, 19 Nov 2025 22:00:13 -0600 Subject: [PATCH 1/6] find equilibrium height --- equilibrium.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 equilibrium.py diff --git a/equilibrium.py b/equilibrium.py new file mode 100644 index 0000000..759837c --- /dev/null +++ b/equilibrium.py @@ -0,0 +1,13 @@ +import sympy as sp + +g = 9.81 +m = 6 +z = sp.symbols('z') +term1 = (6.167266375e8*((0.4446789110e5)**2))/((0.2466906550e10*z + 0.6886569338e8)**2) +term2 = 0.3042813963e19*z*((0.4446789110e5)**2)/((0.2466906550e10*z + 0.6886569338e8)**3) +equation = sp.Eq(4*(term1 - term2), m*g) +solution = sp.solve(equation, z) +print("Equilibrium position (z):") +for sol in solution: + if sol.is_real and sol > 0: + print(sol.evalf()) \ No newline at end of file From 4c12ff17194c971b3ae11c4cc46515b53f7ac7d7 Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Wed, 19 Nov 2025 22:31:55 -0600 Subject: [PATCH 2/6] half-yoke consts --- equilibrium.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/equilibrium.py b/equilibrium.py index 759837c..8fbacab 100644 --- a/equilibrium.py +++ b/equilibrium.py @@ -3,8 +3,8 @@ import sympy as sp g = 9.81 m = 6 z = sp.symbols('z') -term1 = (6.167266375e8*((0.4446789110e5)**2))/((0.2466906550e10*z + 0.6886569338e8)**2) -term2 = 0.3042813963e19*z*((0.4446789110e5)**2)/((0.2466906550e10*z + 0.6886569338e8)**3) +term1 = (6.167266375e8*((0.2223394555e5)**2))/((0.2466906550e10*z + 0.6886569338e8)**2) +term2 = 0.3042813963e19*z*((0.2223394555e5)**2)/((0.2466906550e10*z + 0.6886569338e8)**3) equation = sp.Eq(4*(term1 - term2), m*g) solution = sp.solve(equation, z) print("Equilibrium position (z):") From b404f6846d09129d291054f9032dd1d69e3a5106 Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Thu, 20 Nov 2025 12:26:54 -0600 Subject: [PATCH 3/6] reorganized and made files symbolic links --- DistanceReporting/DistanceReporting.ino | 6 ++ DistanceReporting/IndSensorMap.cpp | 1 + DistanceReporting/IndSensorMap.hpp | 1 + PIDTesting-2yoke4coil/Controller.cpp | 138 +----------------------- PIDTesting-2yoke4coil/Controller.hpp | 106 +----------------- PIDTesting-2yoke4coil/IndSensorMap.cpp | 33 +----- PIDTesting-2yoke4coil/IndSensorMap.hpp | 40 +------ lib/Controller.cpp | 137 +++++++++++++++++++++++ lib/Controller.hpp | 105 ++++++++++++++++++ lib/IndSensorMap.cpp | 32 ++++++ lib/IndSensorMap.hpp | 39 +++++++ 11 files changed, 325 insertions(+), 313 deletions(-) create mode 100644 DistanceReporting/DistanceReporting.ino create mode 120000 DistanceReporting/IndSensorMap.cpp create mode 120000 DistanceReporting/IndSensorMap.hpp mode change 100644 => 120000 PIDTesting-2yoke4coil/Controller.cpp mode change 100644 => 120000 PIDTesting-2yoke4coil/Controller.hpp mode change 100644 => 120000 PIDTesting-2yoke4coil/IndSensorMap.cpp mode change 100644 => 120000 PIDTesting-2yoke4coil/IndSensorMap.hpp create mode 100644 lib/Controller.cpp create mode 100644 lib/Controller.hpp create mode 100644 lib/IndSensorMap.cpp create mode 100644 lib/IndSensorMap.hpp diff --git a/DistanceReporting/DistanceReporting.ino b/DistanceReporting/DistanceReporting.ino new file mode 100644 index 0000000..a5dbdd7 --- /dev/null +++ b/DistanceReporting/DistanceReporting.ino @@ -0,0 +1,6 @@ +#include + +void setup() { + Serial.begin(57600); + +} \ No newline at end of file diff --git a/DistanceReporting/IndSensorMap.cpp b/DistanceReporting/IndSensorMap.cpp new file mode 120000 index 0000000..82f0b2a --- /dev/null +++ b/DistanceReporting/IndSensorMap.cpp @@ -0,0 +1 @@ +../lib/IndSensorMap.cpp \ No newline at end of file diff --git a/DistanceReporting/IndSensorMap.hpp b/DistanceReporting/IndSensorMap.hpp new file mode 120000 index 0000000..b101f1f --- /dev/null +++ b/DistanceReporting/IndSensorMap.hpp @@ -0,0 +1 @@ +../lib/IndSensorMap.hpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.cpp b/PIDTesting-2yoke4coil/Controller.cpp deleted file mode 100644 index 919ee7e..0000000 --- a/PIDTesting-2yoke4coil/Controller.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#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) * 0.25f; - float eCurr = AvgRef - avg; - - avgError.eDiff = (tDiff == 0.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) ? 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) ? 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/PIDTesting-2yoke4coil/Controller.cpp b/PIDTesting-2yoke4coil/Controller.cpp new file mode 120000 index 0000000..cc87053 --- /dev/null +++ b/PIDTesting-2yoke4coil/Controller.cpp @@ -0,0 +1 @@ +../lib/Controller.cpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/PIDTesting-2yoke4coil/Controller.hpp deleted file mode 100644 index eb5272d..0000000 --- a/PIDTesting-2yoke4coil/Controller.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#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; - - float tDiff; -}; -#endif // CONTROLLER_HPP \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/PIDTesting-2yoke4coil/Controller.hpp new file mode 120000 index 0000000..f5297ba --- /dev/null +++ b/PIDTesting-2yoke4coil/Controller.hpp @@ -0,0 +1 @@ +../lib/Controller.hpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.cpp b/PIDTesting-2yoke4coil/IndSensorMap.cpp deleted file mode 100644 index fdae3cd..0000000 --- a/PIDTesting-2yoke4coil/IndSensorMap.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "IndSensorMap.hpp" -#include -#include - -// Sensor calibration data -IndSensorMap ind0Map = {-8.976076325826309, 913.5463710698101, 0.29767471011439534, 5.6686184386250025, 0.3627635461289861}; -IndSensorMap ind1Map = {-4.831976283950702, 885.9877001844566, 0.2793284618109283, 3.8852507844119217, 0.2389935455347361}; -IndSensorMap ind2Map = {-9.824360913609562, 871.4744633266955, 0.2909366235093304, 4.3307594408159495, 0.2822807132259202}; -IndSensorMap ind3Map = {-13.891292062248292, 990.6819962477331, 0.16376045588859353, -0.074904004740735, 0.17727132893449118}; - -// IndSensor class implementation -IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin) - : consts(calibration), pin(analogPin), oor(false) {} - -// Convert raw analog reading to millimeters using sensor calibration -float IndSensor::toMM(unsigned int raw) { - return consts.C - (1.0 / consts.B) * log(pow((consts.K - consts.A) / ((float)raw - consts.A), consts.v) - 1.0); -} - -// Read sensor directly from pin and convert to millimeters -float IndSensor::readMM() { - unsigned int raw = analogRead(pin); - oor = (raw == 0 || raw > 870); // Update out-of-range flag - mmVal = toMM(raw); - return mmVal; -} - -// Predefined sensor instances -IndSensor indL(ind1Map, A0); -IndSensor indR(ind0Map, A1); -IndSensor indF(ind3Map, A5); -IndSensor indB(ind2Map, A4); diff --git a/PIDTesting-2yoke4coil/IndSensorMap.cpp b/PIDTesting-2yoke4coil/IndSensorMap.cpp new file mode 120000 index 0000000..82f0b2a --- /dev/null +++ b/PIDTesting-2yoke4coil/IndSensorMap.cpp @@ -0,0 +1 @@ +../lib/IndSensorMap.cpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PIDTesting-2yoke4coil/IndSensorMap.hpp deleted file mode 100644 index 0705fd9..0000000 --- a/PIDTesting-2yoke4coil/IndSensorMap.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef IND_SENSOR_MAP_HPP -#define IND_SENSOR_MAP_HPP - -#include - -// Inductive Sensor Mapping Struct -typedef struct IndSensorMap { - float A; - float K; - float B; - float C; - float v; -} IndSensorMap; - -class IndSensor { - public: - bool oor; - float mmVal; - - // Constructor - IndSensor(IndSensorMap calibration, uint8_t analogPin); - // Read sensor directly from pin and convert to millimeters - float readMM(); - - private: - IndSensorMap consts; - uint8_t pin; - - // helper function to convert analog reading to millimeters - float toMM(unsigned int raw); -}; - -// sensor instances -extern IndSensor indL; -extern IndSensor indR; -extern IndSensor indF; -extern IndSensor indB; - -#endif // IND_SENSOR_MAP_HPP diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PIDTesting-2yoke4coil/IndSensorMap.hpp new file mode 120000 index 0000000..b101f1f --- /dev/null +++ b/PIDTesting-2yoke4coil/IndSensorMap.hpp @@ -0,0 +1 @@ +../lib/IndSensorMap.hpp \ No newline at end of file diff --git a/lib/Controller.cpp b/lib/Controller.cpp new file mode 100644 index 0000000..919ee7e --- /dev/null +++ b/lib/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) * 0.25f; + float eCurr = AvgRef - avg; + + avgError.eDiff = (tDiff == 0.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) ? 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) ? 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/lib/Controller.hpp b/lib/Controller.hpp new file mode 100644 index 0000000..eb5272d --- /dev/null +++ b/lib/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; + + float tDiff; +}; +#endif // CONTROLLER_HPP \ No newline at end of file diff --git a/lib/IndSensorMap.cpp b/lib/IndSensorMap.cpp new file mode 100644 index 0000000..fdae3cd --- /dev/null +++ b/lib/IndSensorMap.cpp @@ -0,0 +1,32 @@ +#include "IndSensorMap.hpp" +#include +#include + +// Sensor calibration data +IndSensorMap ind0Map = {-8.976076325826309, 913.5463710698101, 0.29767471011439534, 5.6686184386250025, 0.3627635461289861}; +IndSensorMap ind1Map = {-4.831976283950702, 885.9877001844566, 0.2793284618109283, 3.8852507844119217, 0.2389935455347361}; +IndSensorMap ind2Map = {-9.824360913609562, 871.4744633266955, 0.2909366235093304, 4.3307594408159495, 0.2822807132259202}; +IndSensorMap ind3Map = {-13.891292062248292, 990.6819962477331, 0.16376045588859353, -0.074904004740735, 0.17727132893449118}; + +// IndSensor class implementation +IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin) + : consts(calibration), pin(analogPin), oor(false) {} + +// Convert raw analog reading to millimeters using sensor calibration +float IndSensor::toMM(unsigned int raw) { + return consts.C - (1.0 / consts.B) * log(pow((consts.K - consts.A) / ((float)raw - consts.A), consts.v) - 1.0); +} + +// Read sensor directly from pin and convert to millimeters +float IndSensor::readMM() { + unsigned int raw = analogRead(pin); + oor = (raw == 0 || raw > 870); // Update out-of-range flag + mmVal = toMM(raw); + return mmVal; +} + +// Predefined sensor instances +IndSensor indL(ind1Map, A0); +IndSensor indR(ind0Map, A1); +IndSensor indF(ind3Map, A5); +IndSensor indB(ind2Map, A4); diff --git a/lib/IndSensorMap.hpp b/lib/IndSensorMap.hpp new file mode 100644 index 0000000..0705fd9 --- /dev/null +++ b/lib/IndSensorMap.hpp @@ -0,0 +1,39 @@ +#ifndef IND_SENSOR_MAP_HPP +#define IND_SENSOR_MAP_HPP + +#include + +// Inductive Sensor Mapping Struct +typedef struct IndSensorMap { + float A; + float K; + float B; + float C; + float v; +} IndSensorMap; + +class IndSensor { + public: + bool oor; + float mmVal; + + // Constructor + IndSensor(IndSensorMap calibration, uint8_t analogPin); + // Read sensor directly from pin and convert to millimeters + float readMM(); + + private: + IndSensorMap consts; + uint8_t pin; + + // helper function to convert analog reading to millimeters + float toMM(unsigned int raw); +}; + +// sensor instances +extern IndSensor indL; +extern IndSensor indR; +extern IndSensor indF; +extern IndSensor indB; + +#endif // IND_SENSOR_MAP_HPP From 1e9fcfa1af5ac93e812c6d8127f14784f8d454ff Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Thu, 20 Nov 2025 15:13:19 -0600 Subject: [PATCH 4/6] 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 From f3293609f0ada037fd394428b60b9e9184a2b363 Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Thu, 20 Nov 2025 15:21:02 -0600 Subject: [PATCH 5/6] changed K definition to only Kp --- lib/Controller.cpp | 2 +- lib/Controller.hpp | 2 +- lib/PseudoSensorControl.cpp | 2 +- lib/PseudoSensorControl.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/lib/Controller.cpp b/lib/Controller.cpp index 919ee7e..670c2e0 100644 --- a/lib/Controller.cpp +++ b/lib/Controller.cpp @@ -100,7 +100,7 @@ void FullController::FBControl() { 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); + return (int)constrain(constants.kp*errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff, -(float)CAP,(float)CAP); } int16_t FullController::slewLimit(int16_t target, int16_t prev) { diff --git a/lib/Controller.hpp b/lib/Controller.hpp index eb5272d..e878577 100644 --- a/lib/Controller.hpp +++ b/lib/Controller.hpp @@ -17,7 +17,7 @@ #define CAP 200 typedef struct Constants { - float K; + float kp; float ki; float kd; } Constants; diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index b323bc0..3b68923 100644 --- a/lib/PseudoSensorControl.cpp +++ b/lib/PseudoSensorControl.cpp @@ -59,7 +59,7 @@ void PseudoSensorController::control() { 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); + return (int)constrain(constants.kp*errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff, -(float)CAP,(float)CAP); } int16_t PseudoSensorController::slewLimit(int16_t target, int16_t prev) { diff --git a/lib/PseudoSensorControl.hpp b/lib/PseudoSensorControl.hpp index 95ad9d2..457b58a 100644 --- a/lib/PseudoSensorControl.hpp +++ b/lib/PseudoSensorControl.hpp @@ -26,7 +26,7 @@ extern PinPair pinMap[4]; #define CAP 200 typedef struct Constants { - float K; + float kp; float ki; float kd; } Constants; From 6865306d9c2cc08e700d6c2047f8c8805adf766b Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Thu, 20 Nov 2025 15:22:53 -0600 Subject: [PATCH 6/6] Pre-test 1 --- PseudoSensorControl/PseudoSensorControl.ino | 4 ++-- lib/PseudoSensorControl.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/PseudoSensorControl/PseudoSensorControl.ino b/PseudoSensorControl/PseudoSensorControl.ino index 50fd073..1d17bc0 100644 --- a/PseudoSensorControl/PseudoSensorControl.ino +++ b/PseudoSensorControl/PseudoSensorControl.ino @@ -4,8 +4,8 @@ float refs[4] = {10.83,10.83,10.83,10.83}; -Constants repelling = {40, 0.01, 7}; -Constants attracting = {20, 0.01, 20}; +Constants repelling = {10000, 0, 50000}; +Constants attracting = {10000, 0, 50000}; K_MAP consts = {repelling, attracting}; diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index 3b68923..18f7f94 100644 --- a/lib/PseudoSensorControl.cpp +++ b/lib/PseudoSensorControl.cpp @@ -39,10 +39,10 @@ void PseudoSensorController::sendOutputs() { 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}; + float pseudos[4] = {Front.mmVal + Left.mmVal - avg, // FL + Front.mmVal + Right.mmVal - avg, // FR + Back.mmVal + Left.mmVal - avg, // BL + Back.mmVal + Right.mmVal - avg}; // BR for (uint8_t i = 0; i < 4; i++) { float eCurr = Refs[i] - pseudos[i]; // Above reference is positive error.