Merge branch 'main' of https://github.com/pulipakaa24/guadaloop_lev_control

AdditiveControlCode/Controller.cpp

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

AdditiveControlCode/Controller.hpp

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

AdditiveControlCode/IndSensorMap.cpp

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

AdditiveControlCode/IndSensorMap.hpp

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

AdditiveControlCode/PseudoSensorControl.cpp

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

AdditiveControlCode/PseudoSensorControl.hpp

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

PseudoSensorControl/IndSensorMap.cpp

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

PseudoSensorControl/IndSensorMap.hpp

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

PseudoSensorControl/PseudoSensorControl.cpp

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

PseudoSensorControl/PseudoSensorControl.hpp

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

PseudoSensorControl/PseudoSensorControl.ino

@@ -0,0 +1,62 @@
+#include <Arduino.h>
+#include "IndSensorMap.hpp"
+#include "PseudoSensorControl.hpp"
+
+float refs[4] = {10.83,10.83,10.83,10.83};
+
+Constants repelling = {10000, 0, 50000};
+Constants attracting = {10000, 0, 50000};
+
+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...
+  }
+}

equilibrium.py

@@ -0,0 +1,13 @@
+import sympy as sp
+
+g = 9.81
+m = 6
+z = sp.symbols('z')
+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):")
+for sol in solution:
+    if sol.is_real and sol > 0:
+        print(sol.evalf())

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) {

lib/Controller.hpp

@@ -17,7 +17,7 @@
 #define CAP 200
 
 typedef struct Constants {
-  float K;
+  float kp;
   float ki;
   float kd;
 } Constants;

lib/PseudoSensorControl.cpp

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

lib/PseudoSensorControl.hpp

@@ -0,0 +1,81 @@
+#ifndef PSEUDOSENSORCONTROLLER_HPP
+#define PSEUDOSENSORCONTROLLER_HPP
+
+#include <stdint.h>
+#include <Vector.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
+
+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 kp;
+  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