reorganized and made files symbolic links

lib/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) * 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");
+}

lib/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;
+
+    float tDiff;
+};
+#endif // CONTROLLER_HPP

lib/IndSensorMap.cpp

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

lib/IndSensorMap.hpp

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