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

reorganized and made files symbolic links

Browse Source
This commit is contained in:
Aditya Pulipaka
2025-11-20 12:26:54 -06:00
parent 4c12ff1719
commit b404f6846d
11 changed files with 325 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
DistanceReporting/DistanceReporting.ino Normal file
View File

@@ -0,0 +1,6 @@
#include <Arduino.h>
void setup() {
Serial.begin(57600);
}

1
DistanceReporting/IndSensorMap.cpp Symbolic link
View File

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

1
DistanceReporting/IndSensorMap.hpp Symbolic link
View File

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

137
PIDTesting-2yoke4coil/Controller.cpp
View File

@@ -1,137 +0,0 @@
#include "Controller.hpp"
#include <Arduino.h>
// 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");
}

1
PIDTesting-2yoke4coil/Controller.cpp Symbolic link
View File

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

105
PIDTesting-2yoke4coil/Controller.hpp
View File

@@ -1,105 +0,0 @@
#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP
#include <stdint.h>
#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

1
PIDTesting-2yoke4coil/Controller.hpp Symbolic link
View File

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

32
PIDTesting-2yoke4coil/IndSensorMap.cpp
View File

@@ -1,32 +0,0 @@
#include "IndSensorMap.hpp"
#include <Arduino.h>
#include <math.h>
// 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);

1
PIDTesting-2yoke4coil/IndSensorMap.cpp Symbolic link
View File

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

39
PIDTesting-2yoke4coil/IndSensorMap.hpp
View File

@@ -1,39 +0,0 @@
#ifndef IND_SENSOR_MAP_HPP
#define IND_SENSOR_MAP_HPP
#include <stdint.h>
// 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

1
PIDTesting-2yoke4coil/IndSensorMap.hpp Symbolic link
View File

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

137
lib/Controller.cpp Normal file
View File

@@ -0,0 +1,137 @@
#include "Controller.hpp"
#include <Arduino.h>
// 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");
}

105
lib/Controller.hpp Normal file
View File

@@ -0,0 +1,105 @@
#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP
#include <stdint.h>
#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

32
lib/IndSensorMap.cpp Normal file
View File

@@ -0,0 +1,32 @@
#include "IndSensorMap.hpp"
#include <Arduino.h>
#include <math.h>
// 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);

39
lib/IndSensorMap.hpp Normal file
View File

@@ -0,0 +1,39 @@
#ifndef IND_SENSOR_MAP_HPP
#define IND_SENSOR_MAP_HPP
#include <stdint.h>
// 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