From 28e33651a331f4d80025227048c82cda822d8c2d Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Sat, 22 Nov 2025 14:44:25 -0600 Subject: [PATCH] Implemented 31kHz PWM output --- AdditiveControlCode/AdditiveControlCode.ino | 2 ++ AdditiveControlCode/FastPWM.cpp | 1 + AdditiveControlCode/FastPWM.hpp | 1 + PseudoSensorControl/FastPWM.cpp | 1 + PseudoSensorControl/FastPWM.hpp | 1 + PseudoSensorControl/PseudoSensorControl.ino | 3 ++- lib/Controller.cpp | 9 +++++---- lib/Controller.hpp | 4 ++-- lib/FastPWM.cpp | 11 +++++++++++ lib/FastPWM.hpp | 7 +++++++ lib/PseudoSensorControl.cpp | 21 ++++++++++++++++----- lib/PseudoSensorControl.hpp | 4 ++-- 12 files changed, 51 insertions(+), 14 deletions(-) create mode 120000 AdditiveControlCode/FastPWM.cpp create mode 120000 AdditiveControlCode/FastPWM.hpp create mode 120000 PseudoSensorControl/FastPWM.cpp create mode 120000 PseudoSensorControl/FastPWM.hpp create mode 100644 lib/FastPWM.cpp create mode 100644 lib/FastPWM.hpp diff --git a/AdditiveControlCode/AdditiveControlCode.ino b/AdditiveControlCode/AdditiveControlCode.ino index 48ef5a0..b67d924 100644 --- a/AdditiveControlCode/AdditiveControlCode.ino +++ b/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; diff --git a/AdditiveControlCode/FastPWM.cpp b/AdditiveControlCode/FastPWM.cpp new file mode 120000 index 0000000..b4f3868 --- /dev/null +++ b/AdditiveControlCode/FastPWM.cpp @@ -0,0 +1 @@ +../lib/FastPWM.cpp \ No newline at end of file diff --git a/AdditiveControlCode/FastPWM.hpp b/AdditiveControlCode/FastPWM.hpp new file mode 120000 index 0000000..06aee82 --- /dev/null +++ b/AdditiveControlCode/FastPWM.hpp @@ -0,0 +1 @@ +../lib/FastPWM.hpp \ No newline at end of file diff --git a/PseudoSensorControl/FastPWM.cpp b/PseudoSensorControl/FastPWM.cpp new file mode 120000 index 0000000..b4f3868 --- /dev/null +++ b/PseudoSensorControl/FastPWM.cpp @@ -0,0 +1 @@ +../lib/FastPWM.cpp \ No newline at end of file diff --git a/PseudoSensorControl/FastPWM.hpp b/PseudoSensorControl/FastPWM.hpp new file mode 120000 index 0000000..06aee82 --- /dev/null +++ b/PseudoSensorControl/FastPWM.hpp @@ -0,0 +1 @@ +../lib/FastPWM.hpp \ No newline at end of file diff --git a/PseudoSensorControl/PseudoSensorControl.ino b/PseudoSensorControl/PseudoSensorControl.ino index 34a39b6..fd536c6 100644 --- a/PseudoSensorControl/PseudoSensorControl.ino +++ b/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; diff --git a/lib/Controller.cpp b/lib/Controller.cpp index 9faefc4..76adbdb 100644 --- a/lib/Controller.cpp +++ b/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() { diff --git a/lib/Controller.hpp b/lib/Controller.hpp index 537dd1e..5291dd5 100644 --- a/lib/Controller.hpp +++ b/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 diff --git a/lib/FastPWM.cpp b/lib/FastPWM.cpp new file mode 100644 index 0000000..3d395a9 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/lib/FastPWM.hpp b/lib/FastPWM.hpp new file mode 100644 index 0000000..dd5de25 --- /dev/null +++ b/lib/FastPWM.hpp @@ -0,0 +1,7 @@ +#ifndef FASTPWM_H +#define FASTPWM_H +#include + +void setupFastPWM(); + +#endif \ No newline at end of file diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index 26eff6a..1e97cab 100644 --- a/lib/PseudoSensorControl.cpp +++ b/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() { diff --git a/lib/PseudoSensorControl.hpp b/lib/PseudoSensorControl.hpp index f10ec3d..119b9bf 100644 --- a/lib/PseudoSensorControl.hpp +++ b/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