ts changes

AdditiveControlCode/AdditiveControlCode.ino

@@ -3,21 +3,21 @@
 #include "Controller.hpp"
 
 // K, Ki, Kd Constants
-Constants repelling = {40, 0.01, 7};
+Constants repelling = {1000, 0, 10000};
-Constants attracting = {20, 0.01, 20};
+Constants attracting = {1000, 0, 10000};
 
-Constants RollLeftUp = {30, 0.01, 15};
+Constants RollLeftUp = {500, 0, 10000};
-Constants RollLeftDown = {30, 0.01, 15};
+Constants RollLeftDown = {500, 0, 10000};
 
-Constants RollFrontUp = {30, 0.01, 15};
+Constants RollFrontUp = {500, 0, 10000};
-Constants RollFrontDown = {30, 0.01, 15};
+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.

AdditiveControlCode/PseudoSensorControl.cpp

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

AdditiveControlCode/PseudoSensorControl.hpp

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

PseudoSensorControl/PseudoSensorControl.ino

@@ -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 repelling = {250, 0, 20000};
-Constants attracting = {10000, 0, 50000};
+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;

lib/Controller.cpp

@@ -4,9 +4,7 @@
 // CONTROLLER CONSTANTS
 float MAX_INTEGRAL_TERM = 1e4;
 
-void FullController::update(float tDiff) {
+void FullController::update() {
-  this->tDiff = tDiff; // store time step for use in differential and integral portions
-
   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;
+  FLPWM = constrain(avgPWM + LDiffPWM + FDiffPWM, -CAP, CAP);
-  int16_t blTarget = avgPWM + LDiffPWM + BDiffPWM;
+  BLPWM = constrain(avgPWM + LDiffPWM + BDiffPWM, -CAP, CAP);
-  int16_t frTarget = avgPWM + RDiffPWM + FDiffPWM;
+  FRPWM = constrain(avgPWM + RDiffPWM + FDiffPWM, -CAP, CAP);
-  int16_t brTarget = avgPWM + RDiffPWM + BDiffPWM;
+  BRPWM = constrain(avgPWM + RDiffPWM + BDiffPWM, -CAP, CAP);
 
-  FLPWM = slewLimit(flTarget, FLPrev);
+  // FLPWM = avgPWM;
-  BLPWM = slewLimit(blTarget, BLPrev);
+  // BLPWM = avgPWM;
-  FRPWM = slewLimit(frTarget, FRPrev);
+  // FRPWM = avgPWM;
-  BRPWM = slewLimit(brTarget, BRPrev);
+  // BRPWM = avgPWM;
-
-  FLPrev = FLPWM;
-  BLPrev = BLPWM;
-  FRPrev = FRPWM;
-  BRPrev = BRPWM;
 }
 
 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.eDiff = eCurr - avgError.e;
-  avgError.eInt += eCurr * tDiff;
+  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.eDiff = eCurr - LRDiffErr.e;
-  LRDiffErr.eInt += eCurr * tDiff;
+
+  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.eDiff = eCurr - FBDiffErr.e;
-  FBDiffErr.eInt += eCurr * tDiff;
+
+  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);

lib/Controller.hpp

@@ -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),
+      FBDiffErr({0,0,0}), oor(false), outputOn(false) {}
-      FLPrev(0), BLPrev(0), FRPrev(0), BRPrev(0), slewRateLimit(slewRate) {}
 
-    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

lib/IndSensorMap.cpp

@@ -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;

lib/PseudoSensorControl.cpp

@@ -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); // rise over run
+    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].e = eCurr;
 
     PWMs[i] = pwmFunc(Consts, errors[i]);

lib/PseudoSensorControl.hpp

@@ -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;