Implemented 31kHz PWM output

AdditiveControlCode/AdditiveControlCode.ino

@@ -2,6 +2,7 @@
 #include "IndSensorMap.hpp"
 #include "Controller.hpp"
 #include "ADC.hpp"
+#include "FastPWM.hpp"
 
 // K, Ki, Kd Constants
 Constants repelling = {1000, 0, 10000};
@@ -50,6 +51,7 @@ int ON = 0;
 void setup() {
   Serial.begin(115200);
   setupADC();
+  setupFastPWM();
 
   indL.alpha = alphaVal;
   indR.alpha = alphaVal;

AdditiveControlCode/FastPWM.cpp

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

AdditiveControlCode/FastPWM.hpp

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

PseudoSensorControl/FastPWM.cpp

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

PseudoSensorControl/FastPWM.hpp

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

PseudoSensorControl/PseudoSensorControl.ino

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

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