restructuring

2026-03-07 14:10:13 -06:00
parent 6d36cb4bab
commit 2f8e38f339
28 changed files with 0 additions and 0 deletions
--- a/embedded/lib/ADC.cpp
+++ b/embedded/lib/ADC.cpp
@@ -0,0 +1,46 @@
+#include "ADC.hpp"
+
+#define NUM_PINS 4
+
+const uint8_t adc_pins[] = {0, 1, 4, 5}; // A0, A1, A4, A5
+
+volatile uint16_t adc_results[NUM_PINS];
+volatile uint8_t current_channel_index = 0;
+
+void setupADC() {
+  // 1. Set Reference to AVCC (5V)
+  // REFS0 = 1, REFS1 = 0
+  ADMUX = (1 << REFS0);
+
+  // 2. Set ADC Prescaler to 128 (16MHz / 128 = 125KHz)
+  // Good balance of speed and accuracy. 
+  // Bits: ADPS2, ADPS1, ADPS0
+  ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
+
+  // 3. Enable ADC and Enable ADC Interrupt
+  ADCSRA |= (1 << ADEN) | (1 << ADIE);
+
+  // 4. Set initial channel to the first pin in our list
+  ADMUX = (ADMUX & 0xF0) | (adc_pins[0] & 0x0F);
+
+  // 5. Start the first conversion!
+  ADCSRA |= (1 << ADSC); 
+}
+
+ISR(ADC_vect) {
+  // 1. Read the result (must read ADCL first, then ADCH, or just use ADC word)
+  adc_results[current_channel_index] = ADC;
+
+  // 2. Increment to next channel
+  current_channel_index++;
+  if (current_channel_index >= NUM_PINS) {
+    current_channel_index = 0;
+  }
+
+  // 3. Switch MUX to next channel
+  // Clear bottom 4 bits of ADMUX, then OR in the new pin number
+  ADMUX = (ADMUX & 0xF0) | (adc_pins[current_channel_index] & 0x0F);
+
+  // 4. Start next conversion
+  ADCSRA |= (1 << ADSC);
+}
--- a/embedded/lib/ADC.hpp
+++ b/embedded/lib/ADC.hpp
@@ -0,0 +1,11 @@
+#ifndef ADC_H
+#define ADC_H
+
+#include <Arduino.h>
+#include <util/atomic.h>
+
+extern volatile uint16_t adc_results[];
+
+void setupADC();
+
+#endif
--- a/embedded/lib/Controller.cpp
+++ b/embedded/lib/Controller.cpp
@@ -0,0 +1,181 @@
+#include "Controller.hpp"
+#include <Arduino.h>
+#include <math.h>
+
+// ── Integral windup limit ────────────────────────────────────
+static const float MAX_INTEGRAL = 1e4f;
+
+// ── Feedforward LUT in PROGMEM ───────────────────────────────
+// Generated by gen_ff_lut.py  (pod 9.4 kg, R=1.1Ω, V=12V, 3–20 mm)
+// Positive = repelling, Negative = attracting
+static const int16_t FF_PWM_LUT[FF_LUT_SIZE] PROGMEM = {
+   238,  238,  238,  238,  238,  238,  238,  238,
+   238,  238,  238,  238,  238,  238,  238,  238,
+   238,  238,  234,  219,  204,  188,  172,  157,
+   141,  125,  109,   93,   77,   61,   45,   29,
+    13,   -3,  -19,  -35,  -51,  -67,  -84, -100,
+  -116, -133, -150, -166, -183, -200, -217, -234,
+  -250, -250, -250, -250, -250, -250, -250, -250,
+  -250, -250, -250, -250, -250, -250, -250, -250
+};
+
+// ── Constructor ──────────────────────────────────────────────
+FullController::FullController(
+    IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
+    PIDGains hGains, PIDGains rGains, PIDGains pGains,
+    float avgRef, bool useFeedforward)
+  : Left(l), Right(r), Front(f), Back(b),
+    heightGains(hGains), rollGains(rGains), pitchGains(pGains),
+    heightErr({0,0,0}), rollErr({0,0,0}), pitchErr({0,0,0}),
+    AvgRef(avgRef), avg(0), ffEnabled(useFeedforward),
+    oor(false), outputOn(false),
+    FLPWM(0), BLPWM(0), FRPWM(0), BRPWM(0)
+{}
+
+// ── Main update (called each control tick) ───────────────────
+void FullController::update() {
+  // 1. Read all sensors (updates mmVal, oor)
+  Left.readMM();
+  Right.readMM();
+  Front.readMM();
+  Back.readMM();
+
+  oor = Left.oor || Right.oor || Front.oor || Back.oor;
+
+  // 2. Compute average gap (mm)
+  avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
+
+  // 3. Feedforward: base PWM from equilibrium LUT
+  int16_t ffPWM = ffEnabled ? feedforward(avg) : 0;
+
+  // 4. Height PID: error = reference - average gap
+  float heightE = AvgRef - avg;
+  heightErr.eDiff = heightE - heightErr.e;
+  if (!oor) {
+    heightErr.eInt += heightE;
+    heightErr.eInt = constrain(heightErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  heightErr.e = heightE;
+  int16_t heightPWM = pidCompute(heightGains, heightErr, CAP);
+
+  // 5. Roll PID: angle-based error (matches simulation)
+  //    roll_angle = asin((left - right) / y_distance)
+  //    error = -roll_angle  (drive roll toward zero)
+  float rollRatio = (Left.mmVal - Right.mmVal) / Y_DISTANCE_MM;
+  rollRatio = constrain(rollRatio, -1.0f, 1.0f);
+  float rollAngle = asinf(rollRatio);
+  float rollE = -rollAngle;
+
+  rollErr.eDiff = rollE - rollErr.e;
+  if (!oor) {
+    rollErr.eInt += rollE;
+    rollErr.eInt = constrain(rollErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  rollErr.e = rollE;
+  int16_t rollPWM = pidCompute(rollGains, rollErr, CAP / 2);
+
+  // 6. Pitch PID: angle-based error (matches simulation)
+  //    pitch_angle = asin((back - front) / x_distance)
+  //    error = -pitch_angle  (drive pitch toward zero)
+  float pitchRatio = (Back.mmVal - Front.mmVal) / X_DISTANCE_MM;
+  pitchRatio = constrain(pitchRatio, -1.0f, 1.0f);
+  float pitchAngle = asinf(pitchRatio);
+  float pitchE = -pitchAngle;
+
+  pitchErr.eDiff = pitchE - pitchErr.e;
+  if (!oor) {
+    pitchErr.eInt += pitchE;
+    pitchErr.eInt = constrain(pitchErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  pitchErr.e = pitchE;
+  int16_t pitchPWM = pidCompute(pitchGains, pitchErr, CAP / 2);
+
+  // 7. Mix outputs (same sign convention as simulation)
+  //    FL: ff + height - roll - pitch
+  //    FR: ff + height + roll - pitch
+  //    BL: ff + height - roll + pitch
+  //    BR: ff + height + roll + pitch
+  FLPWM = constrain(ffPWM + heightPWM - rollPWM - pitchPWM, -CAP, CAP);
+  FRPWM = constrain(ffPWM + heightPWM + rollPWM - pitchPWM, -CAP, CAP);
+  BLPWM = constrain(ffPWM + heightPWM - rollPWM + pitchPWM, -CAP, CAP);
+  BRPWM = constrain(ffPWM + heightPWM + rollPWM + pitchPWM, -CAP, CAP);
+}
+
+// ── PID compute (single symmetric set of gains) ─────────────
+int16_t FullController::pidCompute(PIDGains& gains, PIDState& state, float maxOutput) {
+  if (oor) return 0;
+  float out = gains.kp * state.e
+            + gains.ki * state.eInt
+            + gains.kd * state.eDiff;
+  return (int16_t)constrain(out, -maxOutput, maxOutput);
+}
+
+// ── Feedforward: linearly interpolate PROGMEM LUT ────────────
+int16_t FullController::feedforward(float gapMM) {
+  if (gapMM <= FF_GAP_MIN) return (int16_t)pgm_read_word(&FF_PWM_LUT[0]);
+  if (gapMM >= FF_GAP_MAX) return (int16_t)pgm_read_word(&FF_PWM_LUT[FF_LUT_SIZE - 1]);
+
+  float idx_f = (gapMM - 1.0 - FF_GAP_MIN) / FF_GAP_STEP;
+  uint8_t idx = (uint8_t)idx_f;
+  if (idx >= FF_LUT_SIZE - 1) idx = FF_LUT_SIZE - 2;
+
+  int16_t v0 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx]);
+  int16_t v1 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx + 1]);
+  float frac = idx_f - (float)idx;
+
+  return (int16_t)(v0 + frac * (v1 - v0));
+}
+
+// ── Zero all PWMs ────────────────────────────────────────────
+void FullController::zeroPWMs() {
+  FLPWM = 0;
+  BLPWM = 0;
+  FRPWM = 0;
+  BRPWM = 0;
+}
+
+// ── Send PWM values to hardware ──────────────────────────────
+void FullController::sendOutputs() {
+  if (!outputOn) {
+    zeroPWMs();
+  }
+
+  // Direction: LOW = repelling (positive PWM), HIGH = attracting (negative PWM)
+  // Using direct register writes for fast PWM mode set by setupFastPWM()
+  digitalWrite(dirFL, FLPWM < 0);
+  OCR2A = abs(FLPWM); // Pin 11 → Timer 2A
+  digitalWrite(dirBL, BLPWM < 0);
+  OCR1A = abs(BLPWM); // Pin 9  → Timer 1A
+  digitalWrite(dirFR, FRPWM < 0);
+  OCR2B = abs(FRPWM); // Pin 3  → Timer 2B
+  digitalWrite(dirBR, BRPWM < 0);
+  OCR1B = abs(BRPWM); // Pin 10 → Timer 1B
+}
+
+// ── Serial report ────────────────────────────────────────────
+void FullController::report() {
+  // CSV: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
+  Serial.print(Left.mmVal);  Serial.print(',');
+  Serial.print(Right.mmVal); Serial.print(',');
+  Serial.print(Front.mmVal); Serial.print(',');
+  Serial.print(Back.mmVal);  Serial.print(',');
+  Serial.print(avg);         Serial.print(',');
+  Serial.print(FLPWM);      Serial.print(',');
+  Serial.print(BLPWM);      Serial.print(',');
+  Serial.print(FRPWM);      Serial.print(',');
+  Serial.print(BRPWM);      Serial.print(',');
+  Serial.println(outputOn);
+}
+
+// ── Runtime tuning methods ───────────────────────────────────
+void FullController::updateHeightPID(PIDGains gains) { heightGains = gains; }
+void FullController::updateRollPID(PIDGains gains)   { rollGains   = gains; }
+void FullController::updatePitchPID(PIDGains gains)  { pitchGains  = gains; }
+
+void FullController::updateReference(float avgReference) {
+  AvgRef = avgReference;
+}
+
+void FullController::setFeedforward(bool enabled) {
+  ffEnabled = enabled;
+}
--- a/embedded/lib/Controller.hpp
+++ b/embedded/lib/Controller.hpp
@@ -0,0 +1,99 @@
+#ifndef CONTROLLER_HPP
+#define CONTROLLER_HPP
+
+#include <stdint.h>
+#include <avr/pgmspace.h>
+#include "IndSensorMap.hpp"
+
+// ── Pin Mapping ──────────────────────────────────────────────
+#define dirBL 2
+#define pwmBL 3
+#define dirBR 4
+#define pwmBR 10
+#define pwmFL 11
+#define dirFL 7
+#define dirFR 8
+#define pwmFR 9
+
+// ── Output Cap ───────────────────────────────────────────────
+#define CAP 250   // Max PWM magnitude (0-255 Arduino range)
+
+// ── PID Gains (single set per loop — matches simulation) ────
+typedef struct PIDGains {
+  float kp;
+  float ki;
+  float kd;
+} PIDGains;
+
+// ── PID Error State ──────────────────────────────────────────
+typedef struct PIDState {
+  float e;        // Current error
+  float eDiff;    // Derivative of error (e[k] - e[k-1])
+  float eInt;     // Integral of error (accumulated)
+} PIDState;
+
+// ── Feedforward LUT (PROGMEM) ────────────────────────────────
+// Generated by gen_ff_lut.py from MaglevPredictor model
+// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
+// Positive = repelling, Negative = attracting
+// Beyond ~16mm: clamped to -CAP (no equilibrium exists)
+#define FF_LUT_SIZE 64
+#define FF_GAP_MIN  3.0f
+#define FF_GAP_MAX  20.0f
+#define FF_GAP_STEP 0.269841f
+
+// ── Geometry (mm, matching simulation) ───────────────────────
+// Sensor-to-sensor distances for angle computation
+#define Y_DISTANCE_MM 101.6f   // Left↔Right sensor spacing (mm)
+#define X_DISTANCE_MM 251.8f   // Front↔Back sensor spacing (mm)
+
+// ── Controller Class ─────────────────────────────────────────
+class FullController {
+public:
+  bool oor;        // Any sensor out-of-range
+  bool outputOn;   // Enable/disable output
+
+  FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
+                 PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
+                 float avgRef, bool useFeedforward);
+
+  void update();
+  void zeroPWMs();
+  void sendOutputs();
+  void report();
+
+  // Runtime tuning
+  void updateHeightPID(PIDGains gains);
+  void updateRollPID(PIDGains gains);
+  void updatePitchPID(PIDGains gains);
+  void updateReference(float avgReference);
+  void setFeedforward(bool enabled);
+
+private:
+  int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
+  int16_t feedforward(float gapMM);
+
+  IndSensor& Left;
+  IndSensor& Right;
+  IndSensor& Front;
+  IndSensor& Back;
+
+  PIDGains heightGains;
+  PIDGains rollGains;
+  PIDGains pitchGains;
+
+  PIDState heightErr;
+  PIDState rollErr;
+  PIDState pitchErr;
+
+  float AvgRef;     // Target gap height (mm)
+  float avg;        // Current average gap (mm)
+  bool  ffEnabled;  // Feedforward on/off
+
+  int16_t FLPWM;
+  int16_t BLPWM;
+  int16_t FRPWM;
+  int16_t BRPWM;
+};
+
+#endif // CONTROLLER_HPP
--- a/embedded/lib/FastPWM.cpp
+++ b/embedded/lib/FastPWM.cpp
@@ -0,0 +1,11 @@
+#include "FastPWM.hpp"
+
+void setupFastPWM() {
+  // Timer 1 (Pins 9 & 10) -> 31.25 kHz
+  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
+  TCCR1B = _BV(CS10); 
+
+  // Timer 2 (Pins 3 & 11) -> 31.25 kHz
+  TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
+  TCCR2B = _BV(CS20); 
+}
--- a/embedded/lib/FastPWM.hpp
+++ b/embedded/lib/FastPWM.hpp
@@ -0,0 +1,7 @@
+#ifndef FASTPWM_H
+#define FASTPWM_H
+#include <Arduino.h>
+
+void setupFastPWM();
+
+#endif
--- a/embedded/lib/IndSensorMap.cpp
+++ b/embedded/lib/IndSensorMap.cpp
@@ -0,0 +1,64 @@
+#include "IndSensorMap.hpp"
+#include <Arduino.h>
+#include <math.h>
+#include "ADC.hpp"
+
+// Sensor calibration data with pre-computed constants
+IndSensorMap ind0Map = {
+  -8.976076325826309, 913.5463710698101, 0.29767471011439534, 5.6686184386250025, 0.3627635461289861,
+  1.0f/0.29767471011439534f, 913.5463710698101f - (-8.976076325826309f)
+};
+IndSensorMap ind1Map = {
+  -4.831976283950702, 885.9877001844566, 0.2793284618109283, 3.8852507844119217, 0.2389935455347361,
+  1.0f/0.2793284618109283f, 885.9877001844566f - (-4.831976283950702f)
+};
+IndSensorMap ind2Map = {
+  -9.824360913609562, 871.4744633266955, 0.2909366235093304, 4.3307594408159495, 0.2822807132259202,
+  1.0f/0.2909366235093304f, 871.4744633266955f - (-9.824360913609562f)
+};
+IndSensorMap ind3Map = {
+  -13.8907146886418, 990.6824637304771, 0.16376005385006073, -0.07513804021312243, 0.1772655198934789,
+  1.0f/0.16376005385006073f, 990.6824637304771f - (-13.8907146886418f)
+};
+
+// IndSensor class implementation
+IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin, float emaAlpha)
+  : consts(calibration), pin(analogPin), alpha(emaAlpha), oor(false), filteredRaw(0) {
+  // Initialize filtered value with first reading
+  filteredRaw = analogRead(pin);
+}
+
+// Convert raw analog reading to millimeters using sensor calibration
+float IndSensor::toMM(uint16_t raw) {
+  // Optimized version using pre-computed constants and faster math functions
+  float raw_minus_A = (float)raw - consts.A;
+  float ratio = consts.K_minus_A / raw_minus_A;
+  float powered = powf(ratio, consts.v);
+  float inside_log = powered - 1.0f;
+  return consts.C - consts.invB * logf(inside_log);
+}
+
+// Read sensor directly from pin and convert to millimeters
+float IndSensor::readMM() {
+  uint8_t index = pin - A0;
+  index = (index > 3) ? index - 2 : index;
+  uint16_t raw;
+
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    raw = constrain(adc_results[index], 0, 900);
+  }
+  
+  // Exponential moving average filter
+  filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;
+  
+  analog = (uint16_t)filteredRaw;
+  oor = (analog < 10 || analog > 870);
+  mmVal = toMM(analog);
+  return mmVal;
+}
+
+// Predefined sensor instances
+IndSensor indF(ind1Map, A0);
+IndSensor indB(ind0Map, A1);
+IndSensor indR(ind3Map, A5);
+IndSensor indL(ind2Map, A4);
--- a/embedded/lib/IndSensorMap.hpp
+++ b/embedded/lib/IndSensorMap.hpp
@@ -0,0 +1,46 @@
+#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;
+  
+  // Pre-computed constants for faster toMM()
+  float invB;      // 1.0 / B
+  float K_minus_A; // K - A
+} IndSensorMap;
+
+class IndSensor {
+  public:
+    bool oor;
+    float mmVal;
+    uint16_t analog;
+    float alpha; // EMA smoothing factor: 0-1, lower = more smoothing
+
+    // Constructor
+    IndSensor(IndSensorMap calibration, uint8_t analogPin, float emaAlpha = 0.3f);
+    // Read sensor directly from pin and convert to millimeters
+    float readMM();
+
+  private:
+    IndSensorMap consts;
+    uint8_t pin;
+    float filteredRaw;
+
+    // helper function to convert analog reading to millimeters
+    float toMM(uint16_t raw);
+};
+
+// sensor instances
+extern IndSensor indL;
+extern IndSensor indR;
+extern IndSensor indF;
+extern IndSensor indB;
+
+#endif // IND_SENSOR_MAP_HPP
--- a/embedded/lib/PseudoSensorControl.cpp
+++ b/embedded/lib/PseudoSensorControl.cpp
@@ -0,0 +1,123 @@
+#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();
+
+  // Using direct register writes to maintain fast PWM mode set by setupFastPWM()
+  // FL: Pin 11 -> Timer 2A
+  digitalWrite(dirFL, PWMs[0] < 0);
+  OCR2A = abs(PWMs[0]);
+  
+  // FR: Pin 3 -> Timer 2B
+  digitalWrite(dirFR, PWMs[1] < 0);
+  OCR2B = abs(PWMs[1]);
+  
+  // BL: Pin 9 -> Timer 1A
+  digitalWrite(dirBL, PWMs[2] < 0);
+  OCR1A = abs(PWMs[2]);
+  
+  // BR: Pin 10 -> Timer 1B
+  digitalWrite(dirBR, PWMs[3] < 0);
+  OCR1B = abs(PWMs[3]);
+}
+
+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];
+    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]);
+  }
+}
+
+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("OOR - Left: ");
+  Serial.print(Left.oor);
+  Serial.print(", Right: ");
+  Serial.print(Right.oor);
+  Serial.print(", Front: ");
+  Serial.print(Front.oor);
+  Serial.print(", Back: ");
+  Serial.print(Back.oor);
+  Serial.print(",\n");
+
+  Serial.print("Overall OOR: ");
+  Serial.println(oor);
+
+  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");
+}
--- a/embedded/lib/PseudoSensorControl.hpp
+++ b/embedded/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 10
+#define pwmFL 11
+#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 255
+
+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