Implemented 31kHz PWM output

lib/Controller.cpp

@@ -40,14 +40,15 @@ void FullController::sendOutputs() {
   }
 
   // The following assumes 0 direction drives repulsion and 1 direction drives attraction.
+  // Using direct register writes to maintain fast PWM mode set by setupFastPWM()
   digitalWrite(dirFL, FLPWM < 0);
-  analogWrite(pwmFL, abs(FLPWM));
+  OCR2A = abs(FLPWM);  // Pin 11 -> Timer 2A
   digitalWrite(dirBL, BLPWM < 0);
-  analogWrite(pwmBL, abs(BLPWM));
+  OCR1A = abs(BLPWM);  // Pin 9 -> Timer 1A
   digitalWrite(dirFR, FRPWM < 0);
-  analogWrite(pwmFR, abs(FRPWM));
+  OCR2B = abs(FRPWM);  // Pin 3 -> Timer 2B
   digitalWrite(dirBR, BRPWM < 0);
-  analogWrite(pwmBR, abs(BRPWM));
+  OCR1B = abs(BRPWM);  // Pin 10 -> Timer 1B
 }
 
 void FullController::avgControl() {

lib/Controller.hpp

@@ -8,8 +8,8 @@
 #define dirFR 2
 #define pwmFR 3
 #define dirBR 4
-#define pwmBR 5
-#define pwmFL 6
+#define pwmBR 10
+#define pwmFL 11
 #define dirFL 7
 #define dirBL 8
 #define pwmBL 9

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

lib/FastPWM.hpp

@@ -0,0 +1,7 @@
+#ifndef FASTPWM_H
+#define FASTPWM_H
+#include <Arduino.h>
+
+void setupFastPWM();
+
+#endif

lib/PseudoSensorControl.cpp

@@ -30,11 +30,22 @@ void PseudoSensorController::zeroPWMs() {
 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]));
-  }
+  // 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() {

lib/PseudoSensorControl.hpp

@@ -9,8 +9,8 @@
 #define dirFR 2
 #define pwmFR 3
 #define dirBR 4
-#define pwmBR 5
-#define pwmFL 6
+#define pwmBR 10
+#define pwmFL 11
 #define dirFL 7
 #define dirBL 8
 #define pwmBL 9