adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ts changes

Browse Source
This commit is contained in:
Aditya Pulipaka
2025-11-22 13:29:00 -06:00
parent f1fea28d73
commit 71092ec86f
9 changed files with 61 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
AdditiveControlCode/AdditiveControlCode.ino
View File

@@ -3,21 +3,21 @@
#include "Controller.hpp"
// K, Ki, Kd Constants
Constants repelling = {40, 0.01, 7};
Constants attracting = {20, 0.01, 20};
Constants repelling = {1000, 0, 10000};
Constants attracting = {1000, 0, 10000};
Constants RollLeftUp = {30, 0.01, 15};
Constants RollLeftDown = {30, 0.01, 15};
Constants RollLeftUp = {500, 0, 10000};
Constants RollLeftDown = {500, 0, 10000};
Constants RollFrontUp = {30, 0.01, 15};
Constants RollFrontDown = {30, 0.01, 15};
Constants RollFrontUp = {500, 0, 10000};
Constants RollFrontDown = {500, 0, 10000};
// Reference values for average dist,
float avgRef = 21.0; // TBD: what is our equilibrium height with this testrig?
float avgRef = 12.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 FBDiffRef = 2; // 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)
float slewRateLimit = 10000.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.
@@ -44,7 +44,7 @@ const int dt_micros = 1e6/sampling_rate;
int ON = 0;
void setup() {
Serial.begin(57600);
Serial.begin(115200);
tprior = micros();
@@ -70,7 +70,7 @@ void loop() {
tDiffMicros = micros() - tprior;
if (tDiffMicros >= dt_micros){
controller.update((float)tDiffMicros / (float)1e6);
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.

1
AdditiveControlCode/PseudoSensorControl.cpp
View File

@@ -1 +0,0 @@
../lib/PseudoSensorControl.cpp

1
AdditiveControlCode/PseudoSensorControl.hpp
View File

@@ -1 +0,0 @@
../lib/PseudoSensorControl.hpp

10
PseudoSensorControl/PseudoSensorControl.ino
View File

@@ -2,10 +2,10 @@
#include "IndSensorMap.hpp"
#include "PseudoSensorControl.hpp"
float refs[4] = {14,14,14,14};
float refs[4] = {12.9,12.3,12.6,12};
Constants repelling = {10000, 0, 50000};
Constants attracting = {10000, 0, 50000};
Constants repelling = {250, 0, 20000};
Constants attracting = {250, 0, 20000};
K_MAP consts = {repelling, attracting};
@@ -34,7 +34,7 @@ const int dt_micros = 1e6/sampling_rate;
int ON = 0;
void setup() {
Serial.begin(57600);
Serial.begin(115200);
indL.alpha = alphaVal;
indR.alpha = alphaVal;
@@ -54,7 +54,7 @@ void loop() {
char c = Serial.read();
while(Serial.available()) Serial.read(); // flush remaining
controller.outputOn = (c != '0');
controller.outputOn = (c != '0' && c != 'r'); // planning to add r command to set refernce or smth
}
tDiffMicros = micros() - tprior;

64
lib/Controller.cpp
View File

@@ -4,9 +4,7 @@
// 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
void FullController::update() {
Left.readMM();
Right.readMM();
Front.readMM();
@@ -18,20 +16,15 @@ void FullController::update(float tDiff) {
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 = 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);
FLPWM = slewLimit(flTarget, FLPrev);
BLPWM = slewLimit(blTarget, BLPrev);
FRPWM = slewLimit(frTarget, FRPrev);
BRPWM = slewLimit(brTarget, BRPrev);
FLPrev = FLPWM;
BLPrev = BLPWM;
FRPrev = FRPWM;
BRPrev = BRPWM;
// FLPWM = avgPWM;
// BLPWM = avgPWM;
// FRPWM = avgPWM;
// BRPWM = avgPWM;
}
void FullController::zeroPWMs() {
@@ -58,12 +51,14 @@ void FullController::sendOutputs() {
}
void FullController::avgControl() {
float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
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.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);
@@ -74,9 +69,13 @@ void FullController::LRControl() {
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.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);
@@ -88,9 +87,13 @@ void FullController::FBControl() {
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.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);
@@ -103,13 +106,6 @@ int16_t FullController::pwmFunc(K_MAP consts, Errors errs) {
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) {
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);
@@ -120,6 +116,8 @@ void FullController::report() {
Serial.print("mm, Back: ");
Serial.print(Back.mmVal);
Serial.print("mm,\n");
Serial.print("AVG - ");
Serial.println(avg);
Serial.print("PWMS - FL_PWM: ");
Serial.print(FLPWM);

16
lib/Controller.hpp
View File

@@ -14,7 +14,7 @@
#define dirBL 8
#define pwmBL 9
#define CAP 200
#define CAP 250
typedef struct Constants {
float kp;
@@ -49,10 +49,9 @@ class FullController {
: 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) {}
FBDiffErr({0,0,0}), oor(false), outputOn(false) {}
void update(float tDiff);
void update();
void zeroPWMs();
void sendOutputs();
void report();
@@ -80,7 +79,7 @@ class FullController {
float AvgRef;
float LRDiffRef;
float FBDiffRef;
float slewRateLimit;
float avg;
int16_t avgPWM;
int16_t LDiffPWM;
@@ -94,12 +93,5 @@ class FullController {
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

1
lib/IndSensorMap.cpp
View File

@@ -23,7 +23,6 @@ float IndSensor::toMM(uint16_t raw) {
// Read sensor directly from pin and convert to millimeters
float IndSensor::readMM() {
uint16_t raw = constrain(analogRead(pin), 0, 900);
Serial.println(raw);
// Exponential moving average filter
filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;

13
lib/PseudoSensorControl.cpp
View File

@@ -45,11 +45,16 @@ void PseudoSensorController::control() {
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.
float eCurr = Refs[i] - pseudos[i];
errors[i].eDiff = (eCurr - errors[i].e);
// Only integrate when not out of range
if (!oor) {
errors[i].eInt += eCurr;
errors[i].eInt = constrain(errors[i].eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
}
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]);

2
lib/PseudoSensorControl.hpp
View File

@@ -23,7 +23,7 @@ typedef struct PinPair {
extern PinPair pinMap[4];
// FL, FR, BL, BR
#define CAP 200
#define CAP 255
typedef struct Constants {
float kp;