Added interrupt-driven non-blocking ADC read. First step in increasing loop frequencies

AdditiveControlCode/ADC.cpp

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

AdditiveControlCode/ADC.hpp

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

AdditiveControlCode/AdditiveControlCode.ino

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include "IndSensorMap.hpp"
 #include "Controller.hpp"
+#include "ADC.hpp"
 
 // K, Ki, Kd Constants
 Constants repelling = {1000, 0, 10000};
@@ -24,6 +25,9 @@ float slewRateLimit = 10000.0; // max PWM change per control cycle (determined b
 // Might be useful for things like jitter or lag.
 #define sampling_rate 1000 // Hz
 
+// EMA filter alpha value (all sensors use same alpha)
+#define alphaVal 0.3f
+
 // ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
 
 unsigned long tprior;
@@ -45,6 +49,12 @@ int ON = 0;
 
 void setup() {
   Serial.begin(115200);
+  setupADC();
+
+  indL.alpha = alphaVal;
+  indR.alpha = alphaVal;
+  indF.alpha = alphaVal;
+  indB.alpha = alphaVal;
 
   tprior = micros();
 

PseudoSensorControl/ADC.cpp

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

PseudoSensorControl/ADC.hpp

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

PseudoSensorControl/PseudoSensorControl.ino

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include "IndSensorMap.hpp"
 #include "PseudoSensorControl.hpp"
+#include "ADC.hpp"
 
 float refs[4] = {12.9,12.3,12.6,12};
 
@@ -36,6 +37,8 @@ int ON = 0;
 void setup() {
   Serial.begin(115200);
 
+  setupADC();
+
   indL.alpha = alphaVal;
   indR.alpha = alphaVal;
   indF.alpha = alphaVal;

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);
+}

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

lib/IndSensorMap.cpp

@@ -1,6 +1,7 @@
 #include "IndSensorMap.hpp"
 #include <Arduino.h>
 #include <math.h>
+#include "ADC.hpp"
 
 // Sensor calibration data
 IndSensorMap ind0Map = {-8.976076325826309, 913.5463710698101, 0.29767471011439534, 5.6686184386250025, 0.3627635461289861};
@@ -22,13 +23,19 @@ 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);
+  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 == 0 || analog > 870);
+  oor = (analog < 10 || analog > 870);
   mmVal = toMM(analog);
   return mmVal;
 }