redesign

Controller.cpp

@@ -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) / 4.0;
+  float eCurr = AvgRef - avg; // assuming up is positive and down is negative
+
+  avgError.eDiff = (tDiff == 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:(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:(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");
+}

Controller.hpp

@@ -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;
+
+    unsigned int tDiff;
+};
+#endif // CONTROLLER_HPP

DualYokeControl (Previous Version).ino

@@ -1,200 +0,0 @@
-#include <Arduino.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
-
-// variables
-
-int dist_raw, tprior, telapsed, pwm, pwm2, dist2_raw;
-bool oor, oor2;
-float dist,ecurr, eprior, derror, ecum, ff,dist2,ecurr2, eprior2, derror2, ecum2, ff2;
-
-#define CAP 200
-
-
-// CONTROLLER CONSTANTS
-float MAX_INTEGRAL_TERM = 1e4;
-
-//// FOR MC 1
-//const float K_f = 50; // gain for when we want to fall (ref > dist or error > 0)
-//const float ki_f = 0.01;
-//const float kd_f = 8; // 25
-//
-//const float K_a = 20;
-//const float ki_a = ki_f;
-//const float kd_a = 10; //30;
-
-typedef struct Constants {
-  float K;
-  float ki;
-  float kd;
-} Constants;
-
-typedef struct K_MAP {
-  Constants falling;
-  Constants attracting;
-} K_MAP;
-
-typedef struct Collective {
-  K_MAP constants;
-  float e;
-  float eDiff;
-  float eInt;
-  float ref;
-} Collective;
-
-Collective collLeft = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 21.0};
-Collective collRight = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 22.0};
-
-int levCollective(Collective collective, bool oor){
-  if (oor){
-    pwm = 0;
-  }
-  else{
-    Constants pidConsts;
-
-    // this means that dist > ref so we gotta attract to track now vv
-    if (collective.e < 0) pidConsts = collective.constants.attracting;
-    // this is falling vv
-    else pidConsts = collective.constants.falling;
-
-    pwm = constrain(pidConsts.K*(collective.e + pidConsts.ki*collective.eInt + pidConsts.kd*collective.eDiff), -CAP,CAP);
-  }
-  return (int)pwm;
-}
-
-#define sampling_rate 1000
-const int dt_micros = 1e6/sampling_rate;
-
-#define LEV_ON
-
-int ON = 0;
-
-void setup() {
-  // put your setup code here, to run once:
-
-  Serial.begin(57600);
-
-  tprior = micros();
-  ecum = 0;
-  ecum2 = 0;
-
-  // positive pwm is A
-  // negative is B
-
-  // ATTRACT IS B  // REPEL IS A
-
-  //when error is negative, I want to attract.
-  send_pwmFL(0);
-  send_pwmFR(0);
-
-}
-
-void loop() {
-  if (Serial.available() > 0) {
-    String inputString = Serial.readStringUntil('\n');  // Read the full input
-    inputString.trim();  // Remove leading/trailing whitespace, including \n and \r
-
-    // Conditional pipeline
-    if (inputString == "0") {
-        ON=0;
-    } 
-    else {
-        ON=1;
-    }
-  }
-  
-  telapsed = micros() - tprior;
-
-  if (telapsed >= dt_micros){
-    // put your main code here, to run repeatedly:
-    indL.read();
-    indR.read();
-    indF.read();
-    indB.read();
-    dist_raw = analogRead(indL);
-    if (dist_raw > 870) oor = true;
-    dist = indToMM(ind0Map, dist_raw); // 189->950, 16->26
-    Serial.print(dist);
-    Serial.print(", ");
-
-    dist2_raw = analogRead(indR);
-    if (dist2_raw > 870) oor2 = true;
-    dist2 = indToMM(ind1Map, dist2_raw);
-    Serial.print(dist2);
-    Serial.print(", ");
-
-    ecurr = ref - dist;
-    derror = ecurr - eprior;
-
-    ecurr2 = ref2 - dist2;
-    derror2 = ecurr2 - eprior2;
-    
-    ecum += ecurr * (telapsed / 1e6);
-    ecum = constrain(ecum, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
-    ecum2 += ecurr2 * (telapsed / 1e6);
-    ecum2 = constrain(ecum2, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
-
-    
-    if (ON) {
-      int collective1 = 
-      send_pwmFL(pwm);
-      send_pwmFR(pwm2);
-      Serial.print(pwm);
-      Serial.print(", ");
-      Serial.print(pwm2);
-      Serial.print(", ");
-    }
-    else {
-      send_pwmFL(0);
-      send_pwmFR(0);
-      Serial.print(0);
-      Serial.print(", ");
-      Serial.print(0);
-      Serial.print(", ");
-    }
-
-    Serial.print(ON);
-    
-    tprior = micros();
-    eprior = ecurr;
-    eprior2 = ecurr2;
-//    //Serial.print(ecurr); Serial.print(","); Serial.print(oor); Serial.print(","); Serial.print(derror); Serial.print(","); Serial.print(pwm); Serial.print(";  "); Serial.print(ecurr2); Serial.print(","); Serial.print(oor2); Serial.print(","); Serial.print(derror2); Serial.print(","); Serial.print(pwm2);
-//    Serial.print(ecurr); Serial.print(","); Serial.print(ecurr2); Serial.print(","); Serial.print(ecum); Serial.print(",");Serial.print(ecum2); Serial.print(",");
-//    
-    Serial.println();
-  }
-
-  //Serial.println(telapsed);
-}
-
-void send_pwmFL(int val){
-  if (val > 0) {
-    digitalWrite(dirFL, LOW);
-  }
-  else{
-    digitalWrite(dirFL,HIGH);
-  }
-  analogWrite(pwmFL,abs(val));
-}
-
-void send_pwmFR(int val){
-  if (val > 0) {
-    digitalWrite(dirFR, LOW);
-  }
-  else{
-    digitalWrite(dirFR,HIGH);
-  }
-
-  analogWrite(pwmFR,abs(val));
-
-}

IndSensorMap.cpp

@@ -18,10 +18,11 @@ float IndSensor::toMM(unsigned int raw) {
 }
 
 // Read sensor directly from pin and convert to millimeters
-float IndSensor::read() {
+float IndSensor::readMM() {
   unsigned int raw = analogRead(pin);
   oor = (raw == 0 || raw > 870);  // Update out-of-range flag
-  return toMM(raw);
+  mmVal = toMM(raw);
+  return mmVal;
 }
 
 // Predefined sensor instances

IndSensorMap.hpp

@@ -1,6 +1,8 @@
 #ifndef IND_SENSOR_MAP_HPP
 #define IND_SENSOR_MAP_HPP
 
+#include <stdint.h>
+
 // Inductive Sensor Mapping Struct
 typedef struct IndSensorMap {
   double A;
@@ -11,16 +13,16 @@ typedef struct IndSensorMap {
 } IndSensorMap;
 
 class IndSensor {
-public:
+  public:
     bool oor;
-  int mmVal;
+    float mmVal;
 
     // Constructor
     IndSensor(IndSensorMap calibration, uint8_t analogPin);
     // Read sensor directly from pin and convert to millimeters
-  float read();
+    float readMM();
 
-private:
+  private:
     IndSensorMap consts;
     uint8_t pin;
 

IndivCoilControl (AI Slop Version).ino.bak

@@ -84,8 +84,8 @@ const float ref2 = 22.0;   // right yoke distance setpoint
 const float K_current  = 1;    // (kept for compatibility)
 const float ki_current = 0;    // (kept for compatibility)
 
-// FOR MC 1 (left yoke): separate gains when falling vs attracting
+// FOR MC 1 (left yoke): separate gains when repelling vs attracting
-const float K_f  = 40;     // falling (error > 0) — push down / reduce lift
+const float K_f  = 40;     // repelling (error > 0) — push down / reduce lift
 const float ki_f = 0.01;
 const float kd_f = 7;
 

lev_control_4pt_small.ino

@@ -0,0 +1,84 @@
+#include <Arduino.h>
+#include "IndSensorMap.hpp"
+#include "Controller.hpp"
+
+// K, Ki, Kd Constants
+Constants repelling = {40, 0.01, 7};
+Constants attracting = {20, 0.01, 20};
+
+Constants RollLeftUp = {30, 0.01, 15};
+Constants RollLeftDown = {30, 0.01, 15};
+
+Constants RollFrontUp = {30, 0.01, 15};
+Constants RollFrontDown = {30, 0.01, 15};
+
+// Reference values for average dist, 
+float avgRef = 21.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 slewRateLimit = 100.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.
+
+// 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;
+
+FullConsts fullConsts = {
+  {repelling, attracting},
+  {RollLeftDown, RollLeftUp},
+  {RollFrontDown, RollFrontUp}
+};
+
+FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef, slewRateLimit);
+
+const int dt_micros = 1e6/sampling_rate;
+
+#define LEV_ON
+
+int ON = 0;
+
+void setup() {
+  Serial.begin(57600);
+
+  tprior = micros();
+
+  pinMode(dirFL, OUTPUT);
+  pinMode(pwmFL, OUTPUT);
+  pinMode(dirBL, OUTPUT);
+  pinMode(pwmBL, OUTPUT);
+  pinMode(dirFR, OUTPUT);
+  pinMode(pwmFR, OUTPUT);
+  pinMode(dirBR, OUTPUT);
+  pinMode(pwmBR, OUTPUT);
+}
+
+void loop() {
+  if (Serial.available() > 0) {
+    String inputString = Serial.readStringUntil('\n');  // Read the full input
+    inputString.trim();  // Remove leading/trailing whitespace, including \n and \r
+
+    // determine whether control is on or off based on serial input.
+    if (inputString == "0") controller.outputOn=0;
+    else controller.outputOn=1;
+  }
+  
+  tDiffMicros = micros() - tprior;
+
+  if (tDiffMicros >= dt_micros){
+    controller.update((float)tDiffMicros / (float)1e6);
+    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...
+  }
+
+  //Serial.println(telapsed);
+}