From 3e4670c6425476d8373ffe3ad084a5444f91c173 Mon Sep 17 00:00:00 2001 From: k-colish Date: Fri, 21 Mar 2025 21:06:32 -0400 Subject: [PATCH 1/9] Created PWM_Module Created library based on MotorInterface --- platformio.ini | 1 + src/Robot/PWM_Module.cpp | 47 +++++++++++++++++++++++++++++++++++++++ src/Robot/PWM_Module.h | 48 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 96 insertions(+) create mode 100644 src/Robot/PWM_Module.cpp create mode 100644 src/Robot/PWM_Module.h diff --git a/platformio.ini b/platformio.ini index fce8421..3e87b71 100644 --- a/platformio.ini +++ b/platformio.ini @@ -19,6 +19,7 @@ lib_deps = fastled/FastLED @ ~3.6.0 https://github.com/PolarRobotics/PR-Lib.git adafruit/Adafruit LIS3MDL@^1.2.4 + adafruit/Adafruit PWM Servo Driver Library @ ^3.0.2 extra_scripts = pre:pio_build_script.py [env:robot] diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp new file mode 100644 index 0000000..e051526 --- /dev/null +++ b/src/Robot/PWM_Module.cpp @@ -0,0 +1,47 @@ +#include +#include + +PWM_Module::PWM_Module() { + if(MotorCount < MAX_NUM_MOTORS) + this->motorIndex = MotorCount++; // assign a servo index to this instance + else + this->motorIndex = 255; + + if(MotorCount == 0){ + pwm.setPWMFreq(PWM_FREQ); // assign frequency to PWM module at first instance + } +} + +uint8_t PWM_Module::attach(int pin, int min = MIN_PWM_US, int max = MAX_PWM_US) { + if(this->motorIndex < MAX_NUM_MOTORS - 1) { + motors[this->motorIndex].pin = pin; // assign this servo a pin + this->min = min; + this->max = max; + pwm.setPWM(this->motorIndex, 0, 0); // No need to set channel + } + return this->motorIndex; +} + +void PWM_Module::write(float pwr) { + pwm.setPWM(this->motorIndex, 0, power2Duty(pwr)); +} + +uint16_t PWM_Module::power2Duty(float power) { + // this can be written in compiler code, but we are trying to save on flash memory + this->tempTimeon = (power + 1) * 500 + 1000; + return tempTimeon / PWM_PERIOD * PWM_MAXDUTY; +} + +void PWM_Module::writelow() { + write(0); +} + +void PWM_Module::displayPinInfo() { + Serial.print(F("Motor: ")); + Serial.print(this->motorIndex); + Serial.print(F(" on Pin #")); + Serial.print(motors[this->motorIndex].pin); + + // Serial.print(F("\r\nDuty Cycle: ")); + // Serial.print(power2Duty(pwr)); +} \ No newline at end of file diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h new file mode 100644 index 0000000..3f05e28 --- /dev/null +++ b/src/Robot/PWM_Module.h @@ -0,0 +1,48 @@ +#pragma once + +#ifndef __PWM_MODULE__ +#define __PWM_MODULE__ + +#include +#include + +#define MAX_NUM_MOTORS 16 + +// input of 0 is 1500 us +#define MAX_PWM_US 2000 // Input of 1 +#define MIN_PWM_US 1000 // Input of -1 + +#define PWM_ADDRESS 0x40 // Default PWM i2c address +#define PWM_RES 12 // Max PWM Resolution is 12 https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf +#define PWM_MAXDUTY (1 << PWM_RES) - 1 +// a Period of 2500us for the sabertooth, gives the st enough time to react to inputs, +// can make this value closer to 2000us if we have issues with the ST not updating fast enough +#define PWM_PERIOD 0.0025 // 2500 us +#define PWM_FREQ 1/PWM_PERIOD + +typedef struct servo{ + uint8_t pin; +}servo_t; + +static servo_t motors[MAX_NUM_MOTORS]; +static uint8_t MotorCount = 0; +Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PWM_ADDRESS); + + +class PWM_Module{ +private: + uint8_t motorIndex; // Index of the Motor + int8_t min; + int8_t max; // maximum PWM value, set based on motor driver (sabertooth is MIN_PWM_US) + uint32_t tempTimeon; + uint16_t power2Duty(float power); + +public: + PWM_Module(); + uint8_t attach(int pin, int min, int max); + void write(float power); + void displayPinInfo(); + void writelow(); +}; + +#endif \ No newline at end of file From 43390af9690e6a1b64ee21e241455a78618077e6 Mon Sep 17 00:00:00 2001 From: k-colish Date: Fri, 28 Mar 2025 21:58:36 -0400 Subject: [PATCH 2/9] Setup for PWM implementation --- platformio.ini | 2 ++ src/PolarRobotics.h | 2 ++ src/Robot/Kicker.h | 1 - src/Robot/MotorControl.h | 8 ++++---- src/Robot/PWM_Module.cpp | 6 +++--- src/Robot/PWM_Module.h | 6 ++++-- src/main.cpp | 2 +- 7 files changed, 16 insertions(+), 11 deletions(-) diff --git a/platformio.ini b/platformio.ini index 3e87b71..0e43a20 100644 --- a/platformio.ini +++ b/platformio.ini @@ -27,6 +27,8 @@ build_src_filter = +<*> - - + -<.pio\libdeps\robot\PR-Lib\src\MotorInterface.h> + -<.pio\libdeps\robot\PR-Lib\src\MotorInterface.cpp> - - - diff --git a/src/PolarRobotics.h b/src/PolarRobotics.h index 75f5eb7..4b87fbf 100644 --- a/src/PolarRobotics.h +++ b/src/PolarRobotics.h @@ -31,6 +31,8 @@ #define M2_PIN 33 // | rightMotor | rightFront | #define M3_PIN 26 // | N/A | leftRear | #define M4_PIN 27 // | N/A | rightRear | +#define PWM0_PIN 0 +#define PWM1_PIN 1 // Pins for special bot features, conveyor, flywheels, etc... #define SPECBOT_PIN1 18 diff --git a/src/Robot/Kicker.h b/src/Robot/Kicker.h index da4e663..caf46fd 100644 --- a/src/Robot/Kicker.h +++ b/src/Robot/Kicker.h @@ -3,7 +3,6 @@ #ifndef KICKER_H #define KICKER_H -#include #include #include // ESP PS5 library, access using global instance `ps5` #include diff --git a/src/Robot/MotorControl.h b/src/Robot/MotorControl.h index 9cb46f2..b1b86d2 100644 --- a/src/Robot/MotorControl.h +++ b/src/Robot/MotorControl.h @@ -1,9 +1,9 @@ #pragma once #include +#include #include -// #include -#include +//#include // Enum for Increasing or Decreasing Flywheel Speed @@ -17,8 +17,8 @@ class MotorControl { float gear_ratio; // the input / output gear ratio // Servo: - MotorInterface Motor; - + //MotorInterface Motor; + PWM_Module Motor; // for ramp float requestedRPM; float lastRampTime; diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index e051526..c4a6176 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -8,7 +8,7 @@ PWM_Module::PWM_Module() { this->motorIndex = 255; if(MotorCount == 0){ - pwm.setPWMFreq(PWM_FREQ); // assign frequency to PWM module at first instance + pwm_test_module.setPWMFreq(PWM_FREQ); // assign frequency to PWM module at first instance } } @@ -17,13 +17,13 @@ uint8_t PWM_Module::attach(int pin, int min = MIN_PWM_US, int max = MAX_PWM_US) motors[this->motorIndex].pin = pin; // assign this servo a pin this->min = min; this->max = max; - pwm.setPWM(this->motorIndex, 0, 0); // No need to set channel + pwm_test_module.setPWM(this->motorIndex, 0, 0); // No need to set channel } return this->motorIndex; } void PWM_Module::write(float pwr) { - pwm.setPWM(this->motorIndex, 0, power2Duty(pwr)); + pwm_test_module.setPWM(this->motorIndex, 0, power2Duty(pwr)); } uint16_t PWM_Module::power2Duty(float power) { diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h index 3f05e28..c041e64 100644 --- a/src/Robot/PWM_Module.h +++ b/src/Robot/PWM_Module.h @@ -19,14 +19,16 @@ // can make this value closer to 2000us if we have issues with the ST not updating fast enough #define PWM_PERIOD 0.0025 // 2500 us #define PWM_FREQ 1/PWM_PERIOD - +#ifndef PWM_MODULE +#define PWM_MODULE +Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); +#endif typedef struct servo{ uint8_t pin; }servo_t; static servo_t motors[MAX_NUM_MOTORS]; static uint8_t MotorCount = 0; -Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PWM_ADDRESS); class PWM_Module{ diff --git a/src/main.cpp b/src/main.cpp index 3096670..db45af6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -128,7 +128,7 @@ void setup() { default: // Assume lineman robot = new Lineman(); drive = new Drive(lineman, driveParams); - drive->setupMotors(M1_PIN, M2_PIN); + drive->setupMotors(PWM0_PIN, PWM1_PIN); } // drive->printSetup(); From cb58758c049e561a74b8bfddda1494f2802b65ac Mon Sep 17 00:00:00 2001 From: rdavies02 Date: Sat, 29 Mar 2025 10:44:23 -0400 Subject: [PATCH 3/9] Working now MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit changed pwm_test_module to use the extern keyword, no more multiple definition issues 👍 --- platformio.ini | 2 +- src/Robot/PWM_Module.cpp | 3 +++ src/Robot/PWM_Module.h | 7 +++---- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/platformio.ini b/platformio.ini index 0e43a20..af4587a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -17,7 +17,7 @@ board_build.mcu = esp32 upload_protocol = esptool lib_deps = fastled/FastLED @ ~3.6.0 - https://github.com/PolarRobotics/PR-Lib.git + ; https://github.com/PolarRobotics/PR-Lib.git adafruit/Adafruit LIS3MDL@^1.2.4 adafruit/Adafruit PWM Servo Driver Library @ ^3.0.2 extra_scripts = pre:pio_build_script.py diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index c4a6176..9822365 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -1,6 +1,9 @@ #include #include +// Define the pwm_test_module variable +Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); + PWM_Module::PWM_Module() { if(MotorCount < MAX_NUM_MOTORS) this->motorIndex = MotorCount++; // assign a servo index to this instance diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h index c041e64..9a1932c 100644 --- a/src/Robot/PWM_Module.h +++ b/src/Robot/PWM_Module.h @@ -19,10 +19,9 @@ // can make this value closer to 2000us if we have issues with the ST not updating fast enough #define PWM_PERIOD 0.0025 // 2500 us #define PWM_FREQ 1/PWM_PERIOD -#ifndef PWM_MODULE -#define PWM_MODULE -Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); -#endif + +extern Adafruit_PWMServoDriver pwm_test_module; // Declare as extern + typedef struct servo{ uint8_t pin; }servo_t; From 118d81738465228efd53a10cdde2c164b65e82b8 Mon Sep 17 00:00:00 2001 From: rdavies02 Date: Mon, 31 Mar 2025 20:20:55 -0400 Subject: [PATCH 4/9] =?UTF-8?q?yes,=20something=20is=20happening,=20certai?= =?UTF-8?q?nly=20an=20esp=20error=20=F0=9F=91=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot/PWM_Module.cpp | 5 ++++- src/Robot/PWM_Module.h | 8 ++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index 9822365..141bdd5 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -2,7 +2,8 @@ #include // Define the pwm_test_module variable -Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); +// Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); +// Adafruit_PWMServoDriver PWM_Module::pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); PWM_Module::PWM_Module() { if(MotorCount < MAX_NUM_MOTORS) @@ -11,6 +12,8 @@ PWM_Module::PWM_Module() { this->motorIndex = 255; if(MotorCount == 0){ + pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); + pwm_test_module.begin(); // initialize the PWM module pwm_test_module.setPWMFreq(PWM_FREQ); // assign frequency to PWM module at first instance } } diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h index 9a1932c..0d3a0dd 100644 --- a/src/Robot/PWM_Module.h +++ b/src/Robot/PWM_Module.h @@ -20,24 +20,22 @@ #define PWM_PERIOD 0.0025 // 2500 us #define PWM_FREQ 1/PWM_PERIOD -extern Adafruit_PWMServoDriver pwm_test_module; // Declare as extern typedef struct servo{ - uint8_t pin; + uint8_t pin; }servo_t; static servo_t motors[MAX_NUM_MOTORS]; static uint8_t MotorCount = 0; - class PWM_Module{ private: + Adafruit_PWMServoDriver pwm_test_module; uint8_t motorIndex; // Index of the Motor int8_t min; int8_t max; // maximum PWM value, set based on motor driver (sabertooth is MIN_PWM_US) uint32_t tempTimeon; uint16_t power2Duty(float power); - public: PWM_Module(); uint8_t attach(int pin, int min, int max); @@ -46,4 +44,6 @@ class PWM_Module{ void writelow(); }; +// extern Adafruit_PWMServoDriver PWM_Module::pwm_test_module; // Declare as extern + #endif \ No newline at end of file From ac4a552dd562b3e4f2597e07c856b2b59db16bdf Mon Sep 17 00:00:00 2001 From: rdavies02 Date: Wed, 16 Apr 2025 21:31:48 -0400 Subject: [PATCH 5/9] No esp errors now :) --- src/Robot/PWM_Module.cpp | 60 +++++++++++++++++++++++----------------- src/Robot/PWM_Module.h | 5 ++-- src/main.cpp | 2 +- 3 files changed, 38 insertions(+), 29 deletions(-) diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index 141bdd5..c239a1d 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -5,48 +5,56 @@ // Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); // Adafruit_PWMServoDriver PWM_Module::pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); +Adafruit_PWMServoDriver* PWM_Module::pwm_test_module = nullptr; + +Adafruit_PWMServoDriver* PWM_Module::getPWMInstance() { + if (pwm_test_module == nullptr) { + pwm_test_module = new Adafruit_PWMServoDriver(PWM_ADDRESS); + pwm_test_module->begin(); + pwm_test_module->setPWMFreq(PWM_FREQ); // Set frequency + } + return pwm_test_module; +} + PWM_Module::PWM_Module() { - if(MotorCount < MAX_NUM_MOTORS) - this->motorIndex = MotorCount++; // assign a servo index to this instance - else - this->motorIndex = 255; - - if(MotorCount == 0){ - pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); - pwm_test_module.begin(); // initialize the PWM module - pwm_test_module.setPWMFreq(PWM_FREQ); // assign frequency to PWM module at first instance - } + if(MotorCount == 0){ + getPWMInstance(); // initialize the PWM module + } + if(MotorCount < MAX_NUM_MOTORS) + this->motorIndex = MotorCount++; // assign a servo index to this instance + else + this->motorIndex = 255; } uint8_t PWM_Module::attach(int pin, int min = MIN_PWM_US, int max = MAX_PWM_US) { - if(this->motorIndex < MAX_NUM_MOTORS - 1) { - motors[this->motorIndex].pin = pin; // assign this servo a pin - this->min = min; - this->max = max; - pwm_test_module.setPWM(this->motorIndex, 0, 0); // No need to set channel - } - return this->motorIndex; + if(this->motorIndex < MAX_NUM_MOTORS - 1) { + motors[this->motorIndex].pin = pin; // assign this servo a pin + this->min = min; + this->max = max; + pwm_test_module->setPWM(this->motorIndex, 0, 0); // No need to set channel + } + return this->motorIndex; } void PWM_Module::write(float pwr) { - pwm_test_module.setPWM(this->motorIndex, 0, power2Duty(pwr)); + pwm_test_module->setPWM(this->motorIndex, 0, power2Duty(pwr)); } uint16_t PWM_Module::power2Duty(float power) { - // this can be written in compiler code, but we are trying to save on flash memory - this->tempTimeon = (power + 1) * 500 + 1000; - return tempTimeon / PWM_PERIOD * PWM_MAXDUTY; + // this can be written in compiler code, but we are trying to save on flash memory + this->tempTimeon = (power + 1) * 500 + 1000; + return tempTimeon / PWM_PERIOD * PWM_MAXDUTY; } void PWM_Module::writelow() { - write(0); + write(0); } void PWM_Module::displayPinInfo() { - Serial.print(F("Motor: ")); - Serial.print(this->motorIndex); - Serial.print(F(" on Pin #")); - Serial.print(motors[this->motorIndex].pin); + Serial.print(F("Motor: ")); + Serial.print(this->motorIndex); + Serial.print(F(" on Pin #")); + Serial.print(motors[this->motorIndex].pin); // Serial.print(F("\r\nDuty Cycle: ")); // Serial.print(power2Duty(pwr)); diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h index 0d3a0dd..91b8855 100644 --- a/src/Robot/PWM_Module.h +++ b/src/Robot/PWM_Module.h @@ -5,6 +5,7 @@ #include #include +#include #define MAX_NUM_MOTORS 16 @@ -20,7 +21,6 @@ #define PWM_PERIOD 0.0025 // 2500 us #define PWM_FREQ 1/PWM_PERIOD - typedef struct servo{ uint8_t pin; }servo_t; @@ -30,18 +30,19 @@ static uint8_t MotorCount = 0; class PWM_Module{ private: - Adafruit_PWMServoDriver pwm_test_module; uint8_t motorIndex; // Index of the Motor int8_t min; int8_t max; // maximum PWM value, set based on motor driver (sabertooth is MIN_PWM_US) uint32_t tempTimeon; uint16_t power2Duty(float power); + static Adafruit_PWMServoDriver* pwm_test_module; // Singleton instance public: PWM_Module(); uint8_t attach(int pin, int min, int max); void write(float power); void displayPinInfo(); void writelow(); + static Adafruit_PWMServoDriver* getPWMInstance(); // Method to get the instance }; // extern Adafruit_PWMServoDriver PWM_Module::pwm_test_module; // Declare as extern diff --git a/src/main.cpp b/src/main.cpp index db45af6..2063d9d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -229,7 +229,7 @@ void loop() { robot->action(); // DEBUGGING: - // drive->printDebugInfo(); // comment this line out to reduce compile time and memory usage + drive->printDebugInfo(); // comment this line out to reduce compile time and memory usage // drive->printCsvInfo(); // prints info to serial monitor in a csv (comma separated value) format // lights.printDebugInfo(); From 7a12a48e04b8f4fbbd71905847aa48c02f74657c Mon Sep 17 00:00:00 2001 From: k-colish Date: Mon, 28 Apr 2025 19:21:15 -0400 Subject: [PATCH 6/9] Update PWM_Module.cpp --- src/Robot/PWM_Module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index c239a1d..aa72745 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -43,7 +43,7 @@ void PWM_Module::write(float pwr) { uint16_t PWM_Module::power2Duty(float power) { // this can be written in compiler code, but we are trying to save on flash memory this->tempTimeon = (power + 1) * 500 + 1000; - return tempTimeon / PWM_PERIOD * PWM_MAXDUTY; + return tempTimeon / (PWM_PERIOD * 1000) * (PWM_MAXDUTY / 1000); } void PWM_Module::writelow() { From b220839657df34008022e0eee402dcee5d175dd6 Mon Sep 17 00:00:00 2001 From: k-colish Date: Mon, 28 Apr 2025 19:22:51 -0400 Subject: [PATCH 7/9] Created test file --- platformio.ini | 1 + src/Robot/PWM_Module.cpp | 6 ++- src/main_test.cpp | 101 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 106 insertions(+), 2 deletions(-) create mode 100644 src/main_test.cpp diff --git a/platformio.ini b/platformio.ini index af4587a..c83a6d0 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,6 +25,7 @@ extra_scripts = pre:pio_build_script.py [env:robot] build_src_filter = +<*> + - - - -<.pio\libdeps\robot\PR-Lib\src\MotorInterface.h> diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index aa72745..ef985f5 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -20,8 +20,10 @@ PWM_Module::PWM_Module() { if(MotorCount == 0){ getPWMInstance(); // initialize the PWM module } - if(MotorCount < MAX_NUM_MOTORS) - this->motorIndex = MotorCount++; // assign a servo index to this instance + if(MotorCount < MAX_NUM_MOTORS){ + this->motorIndex = MotorCount; // assign a servo index to this instance + MotorCount++; + } else this->motorIndex = 255; } diff --git a/src/main_test.cpp b/src/main_test.cpp new file mode 100644 index 0000000..9e1b5a5 --- /dev/null +++ b/src/main_test.cpp @@ -0,0 +1,101 @@ +/*************************************************** + This is an example for our Adafruit 16-channel PWM & Servo driver + Servo test - this will drive 8 servos, one after the other on the + first 8 pins of the PCA9685 + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/products/815 + + These drivers use I2C to communicate, 2 pins are required to + interface. + + Adafruit invests time and resources providing this open source code, + please support Adafruit and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#include +#include +#include + +// called this way, it uses the default address 0x40 +Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); +// you can also call it with a different address you want +//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41); +// you can also call it with a different address and I2C interface +//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire); + +// Depending on your servo make, the pulse width min and max may vary, you +// want these to be as small/large as possible without hitting the hard stop +// for max range. You'll have to tweak them as necessary to match the servos you +// have! +#define SERVOMIN 1000 // This is the 'minimum' pulse length count (out of 4096) +#define SERVOMAX 2000 // This is the 'maximum' pulse length count (out of 4096) +#define USMIN 4000 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 +#define USMAX 8000 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 +#define PWM_PERIOD 0.0025 // 2500 us +#define SERVO_FREQ 1/PWM_PERIOD + +// our servo # counter +uint8_t servonum = 1; +void setup(){ + Serial.begin(115200); + Serial.println("8 channel Servo test!"); + + pwm.begin(); + /* + * In theory the internal oscillator (clock) is 25MHz but it really isn't + * that precise. You can 'calibrate' this by tweaking this number until + * you get the PWM update frequency you're expecting! + * The int.osc. for the PCA9685 chip is a range between about 23-27MHz and + * is used for calculating things like writeMicroseconds() + * Analog servos run at ~50 Hz updates, It is importaint to use an + * oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip. + * 1) Attach the oscilloscope to one of the PWM signal pins and ground on + * the I2C PCA9685 chip you are setting the value for. + * 2) Adjust setOscillatorFrequency() until the PWM update frequency is the + * expected value (50Hz for most ESCs) + * Setting the value here is specific to each individual I2C PCA9685 chip and + * affects the calculations for the PWM update frequency. + * Failure to correctly set the int.osc value will cause unexpected PWM results + */ + pwm.setOscillatorFrequency(27000000); + pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates + + delay(10); +} + +// You can use this function if you'd like to set the pulse length in seconds +// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise! +void setServoPulse(uint8_t n, double pulse) { + double pulselength; + + pulselength = 1000000; // 1,000,000 us per second + pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates + Serial.print(pulselength); Serial.println(" us per period"); + pulselength /= 4096; // 12 bits of resolution + Serial.print(pulselength); Serial.println(" us per bit"); + pulse *= 1000000; // convert input seconds to us + pulse /= pulselength; + Serial.println(pulse); + pwm.setPWM(n, 0, pulse); +} + +void loop() { + // Drive each servo one at a time using setPWM() + Serial.println(servonum); + for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) { + pwm.setPWM(servonum, 0, pulselen); + } + + delay(500); + for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) { + pwm.setPWM(servonum, 0, pulselen); + } + + delay(500); + +} From 2491b6ca541d09a0f7b3556045992edc00ae353d Mon Sep 17 00:00:00 2001 From: k-colish Date: Mon, 28 Apr 2025 21:19:34 -0400 Subject: [PATCH 8/9] PWM is working! Driving works as implemented using MotorInterface --- platformio.ini | 6 ++++++ src/Robot/PWM_Module.cpp | 5 +++-- src/Robot/PWM_Module.h | 4 ++-- src/main_test.cpp | 46 +++++++++++++++++++++++++++++++--------- 4 files changed, 47 insertions(+), 14 deletions(-) diff --git a/platformio.ini b/platformio.ini index c83a6d0..8d04f7d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -34,6 +34,12 @@ build_src_filter = - - + +[env:test] +build_src_filter = + -<*> + + + [env:depairing] lib_deps = fastled/FastLED @ ~3.6.0 diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp index ef985f5..dc0c500 100644 --- a/src/Robot/PWM_Module.cpp +++ b/src/Robot/PWM_Module.cpp @@ -12,6 +12,7 @@ Adafruit_PWMServoDriver* PWM_Module::getPWMInstance() { pwm_test_module = new Adafruit_PWMServoDriver(PWM_ADDRESS); pwm_test_module->begin(); pwm_test_module->setPWMFreq(PWM_FREQ); // Set frequency + pwm_test_module->setOscillatorFrequency(27000000); } return pwm_test_module; } @@ -33,7 +34,7 @@ uint8_t PWM_Module::attach(int pin, int min = MIN_PWM_US, int max = MAX_PWM_US) motors[this->motorIndex].pin = pin; // assign this servo a pin this->min = min; this->max = max; - pwm_test_module->setPWM(this->motorIndex, 0, 0); // No need to set channel + pwm_test_module->setPWM(this->motorIndex, 0, power2Duty(0)); // No need to set channel } return this->motorIndex; } @@ -45,7 +46,7 @@ void PWM_Module::write(float pwr) { uint16_t PWM_Module::power2Duty(float power) { // this can be written in compiler code, but we are trying to save on flash memory this->tempTimeon = (power + 1) * 500 + 1000; - return tempTimeon / (PWM_PERIOD * 1000) * (PWM_MAXDUTY / 1000); + return (tempTimeon / (PWM_PERIOD* 1000000)) * PWM_MAXDUTY; } void PWM_Module::writelow() { diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h index 91b8855..b2b8e18 100644 --- a/src/Robot/PWM_Module.h +++ b/src/Robot/PWM_Module.h @@ -18,8 +18,8 @@ #define PWM_MAXDUTY (1 << PWM_RES) - 1 // a Period of 2500us for the sabertooth, gives the st enough time to react to inputs, // can make this value closer to 2000us if we have issues with the ST not updating fast enough -#define PWM_PERIOD 0.0025 // 2500 us -#define PWM_FREQ 1/PWM_PERIOD +#define PWM_PERIOD 0.002 // 2500 us +#define PWM_FREQ 1/0.0022 // typedef struct servo{ uint8_t pin; diff --git a/src/main_test.cpp b/src/main_test.cpp index 9e1b5a5..5de424e 100644 --- a/src/main_test.cpp +++ b/src/main_test.cpp @@ -33,11 +33,13 @@ Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); // for max range. You'll have to tweak them as necessary to match the servos you // have! #define SERVOMIN 1000 // This is the 'minimum' pulse length count (out of 4096) -#define SERVOMAX 2000 // This is the 'maximum' pulse length count (out of 4096) +#define SERVOMAX 3095 // This is the 'maximum' pulse length count (out of 4096) #define USMIN 4000 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 #define USMAX 8000 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 #define PWM_PERIOD 0.0025 // 2500 us #define SERVO_FREQ 1/PWM_PERIOD +#define PWM_RES 12 +#define PWM_MAXDUTY (1 << PWM_RES) - 1 // our servo # counter uint8_t servonum = 1; @@ -84,18 +86,42 @@ void setServoPulse(uint8_t n, double pulse) { pwm.setPWM(n, 0, pulse); } +uint16_t power2Duty(float power) { + // this can be written in compiler code, but we are trying to save on flash memory + float tempTimeon = (power + 1) * 500 + 1000; + return tempTimeon / (PWM_PERIOD * 1000000) * (PWM_MAXDUTY); +} + void loop() { // Drive each servo one at a time using setPWM() - Serial.println(servonum); - for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) { - pwm.setPWM(servonum, 0, pulselen); - } + // Serial.println(servonum); + // for (uint16_t pulselen = SERVOMIN; pulselen < 2048; pulselen++) { + // pwm.setPWM(servonum, 0, pulselen); + // Serial.print("Pulse Length: "); + // Serial.println(pulselen); + // } + // delay(500); + // for (uint16_t pulselen = 2048; pulselen > SERVOMIN; pulselen--) { + // pwm.setPWM(servonum, 0, pulselen); + // Serial.print("Pulse Length: "); + // Serial.println(pulselen); + // } + - delay(500); - for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) { - pwm.setPWM(servonum, 0, pulselen); - } + // for(float i = -1; i < 1; i+=0.05){ + // Serial.print("i: "); + // Serial.print(i); + // Serial.print(" p2d: "); + // Serial.println(power2Duty(i)); + // pwm.setPWM(1,0,power2Duty(i)); + // delay(100); + // } + // for(float i = 1; i > -1; i-=0.05){ + // pwm.setPWM(1,0,power2Duty(i)); + // delay(100); + // } - delay(500); + power2Duty(0.0f); + // delay(500); } From 5299e311127e5a132a906e3312be0a59ea77f940 Mon Sep 17 00:00:00 2001 From: rdavies02 Date: Wed, 30 Apr 2025 21:30:51 -0400 Subject: [PATCH 9/9] fixed some library issues because PR-Lib/add-pwm-module depends on the Adafruit_PWMServoDriver library, it needs to be included in the .ini file --- platformio.ini | 11 +--- src/Robot/MotorControl.cpp | 4 +- src/Robot/MotorControl.h | 5 +- src/Robot/PWM_Module.cpp | 64 ------------------- src/Robot/PWM_Module.h | 50 --------------- src/main_test.cpp | 127 ------------------------------------- 6 files changed, 6 insertions(+), 255 deletions(-) delete mode 100644 src/Robot/PWM_Module.cpp delete mode 100644 src/Robot/PWM_Module.h delete mode 100644 src/main_test.cpp diff --git a/platformio.ini b/platformio.ini index 8d04f7d..e57a0cb 100644 --- a/platformio.ini +++ b/platformio.ini @@ -17,15 +17,14 @@ board_build.mcu = esp32 upload_protocol = esptool lib_deps = fastled/FastLED @ ~3.6.0 - ; https://github.com/PolarRobotics/PR-Lib.git - adafruit/Adafruit LIS3MDL@^1.2.4 adafruit/Adafruit PWM Servo Driver Library @ ^3.0.2 + https://github.com/PolarRobotics/PR-Lib.git#add-pwm-module #dependent on Adafruit PWM Servo Driver Library + adafruit/Adafruit LIS3MDL@^1.2.4 extra_scripts = pre:pio_build_script.py [env:robot] build_src_filter = +<*> - - - - -<.pio\libdeps\robot\PR-Lib\src\MotorInterface.h> @@ -34,12 +33,6 @@ build_src_filter = - - - -[env:test] -build_src_filter = - -<*> - + - [env:depairing] lib_deps = fastled/FastLED @ ~3.6.0 diff --git a/src/Robot/MotorControl.cpp b/src/Robot/MotorControl.cpp index 37bea59..9fb6a37 100644 --- a/src/Robot/MotorControl.cpp +++ b/src/Robot/MotorControl.cpp @@ -46,8 +46,6 @@ MotorControl::MotorControl() { lastRampTime = millis(); } - - /** * @brief setup the given pin to the next free channel, returns channel number or 255 if failure * @author Rhys Davies @@ -64,7 +62,7 @@ uint8_t MotorControl::setup(int mot_pin, MotorType type, bool has_encoder, float this->enc_a_pin = enc_a_chan_pin, this->enc_b_pin = enc_b_chan_pin; // Calculate the max rpm by multiplying the nominal motor RPM by the gear ratio - this->max_rpm = int(MOTOR_MAX_RPM_ARR[static_cast(this->motor_type)] * this->gear_ratio); + this->max_rpm = uint16_t(MOTOR_MAX_RPM_ARR[static_cast(this->motor_type)] * this->gear_ratio); // call the logic to attach the motor pin and setup, return 255 on an error return Motor.attach(mot_pin, MIN_PWM_US, MAX_PWM_US); diff --git a/src/Robot/MotorControl.h b/src/Robot/MotorControl.h index b1b86d2..3ad5b02 100644 --- a/src/Robot/MotorControl.h +++ b/src/Robot/MotorControl.h @@ -1,8 +1,9 @@ #pragma once #include -#include +// #include #include +#include //#include @@ -42,7 +43,7 @@ class MotorControl { float omega; public: - int max_rpm; // the motor max rpm * the gear ratio + uint16_t max_rpm; // the motor max rpm * the gear ratio MotorControl(); uint8_t setup(int mot_pin, MotorType type = big_ampflow, bool has_encoder = false, float gearRatio = 1, int enc_a_chan_pin = -1, int enc_b_chan_pin = -1); // if no encoder, leave blank, will not attach pins diff --git a/src/Robot/PWM_Module.cpp b/src/Robot/PWM_Module.cpp deleted file mode 100644 index dc0c500..0000000 --- a/src/Robot/PWM_Module.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include -#include - -// Define the pwm_test_module variable -// Adafruit_PWMServoDriver pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); -// Adafruit_PWMServoDriver PWM_Module::pwm_test_module = Adafruit_PWMServoDriver(PWM_ADDRESS); - -Adafruit_PWMServoDriver* PWM_Module::pwm_test_module = nullptr; - -Adafruit_PWMServoDriver* PWM_Module::getPWMInstance() { - if (pwm_test_module == nullptr) { - pwm_test_module = new Adafruit_PWMServoDriver(PWM_ADDRESS); - pwm_test_module->begin(); - pwm_test_module->setPWMFreq(PWM_FREQ); // Set frequency - pwm_test_module->setOscillatorFrequency(27000000); - } - return pwm_test_module; -} - -PWM_Module::PWM_Module() { - if(MotorCount == 0){ - getPWMInstance(); // initialize the PWM module - } - if(MotorCount < MAX_NUM_MOTORS){ - this->motorIndex = MotorCount; // assign a servo index to this instance - MotorCount++; - } - else - this->motorIndex = 255; -} - -uint8_t PWM_Module::attach(int pin, int min = MIN_PWM_US, int max = MAX_PWM_US) { - if(this->motorIndex < MAX_NUM_MOTORS - 1) { - motors[this->motorIndex].pin = pin; // assign this servo a pin - this->min = min; - this->max = max; - pwm_test_module->setPWM(this->motorIndex, 0, power2Duty(0)); // No need to set channel - } - return this->motorIndex; -} - -void PWM_Module::write(float pwr) { - pwm_test_module->setPWM(this->motorIndex, 0, power2Duty(pwr)); -} - -uint16_t PWM_Module::power2Duty(float power) { - // this can be written in compiler code, but we are trying to save on flash memory - this->tempTimeon = (power + 1) * 500 + 1000; - return (tempTimeon / (PWM_PERIOD* 1000000)) * PWM_MAXDUTY; -} - -void PWM_Module::writelow() { - write(0); -} - -void PWM_Module::displayPinInfo() { - Serial.print(F("Motor: ")); - Serial.print(this->motorIndex); - Serial.print(F(" on Pin #")); - Serial.print(motors[this->motorIndex].pin); - - // Serial.print(F("\r\nDuty Cycle: ")); - // Serial.print(power2Duty(pwr)); -} \ No newline at end of file diff --git a/src/Robot/PWM_Module.h b/src/Robot/PWM_Module.h deleted file mode 100644 index b2b8e18..0000000 --- a/src/Robot/PWM_Module.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -#ifndef __PWM_MODULE__ -#define __PWM_MODULE__ - -#include -#include -#include - -#define MAX_NUM_MOTORS 16 - -// input of 0 is 1500 us -#define MAX_PWM_US 2000 // Input of 1 -#define MIN_PWM_US 1000 // Input of -1 - -#define PWM_ADDRESS 0x40 // Default PWM i2c address -#define PWM_RES 12 // Max PWM Resolution is 12 https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf -#define PWM_MAXDUTY (1 << PWM_RES) - 1 -// a Period of 2500us for the sabertooth, gives the st enough time to react to inputs, -// can make this value closer to 2000us if we have issues with the ST not updating fast enough -#define PWM_PERIOD 0.002 // 2500 us -#define PWM_FREQ 1/0.0022 // - -typedef struct servo{ - uint8_t pin; -}servo_t; - -static servo_t motors[MAX_NUM_MOTORS]; -static uint8_t MotorCount = 0; - -class PWM_Module{ -private: - uint8_t motorIndex; // Index of the Motor - int8_t min; - int8_t max; // maximum PWM value, set based on motor driver (sabertooth is MIN_PWM_US) - uint32_t tempTimeon; - uint16_t power2Duty(float power); - static Adafruit_PWMServoDriver* pwm_test_module; // Singleton instance -public: - PWM_Module(); - uint8_t attach(int pin, int min, int max); - void write(float power); - void displayPinInfo(); - void writelow(); - static Adafruit_PWMServoDriver* getPWMInstance(); // Method to get the instance -}; - -// extern Adafruit_PWMServoDriver PWM_Module::pwm_test_module; // Declare as extern - -#endif \ No newline at end of file diff --git a/src/main_test.cpp b/src/main_test.cpp deleted file mode 100644 index 5de424e..0000000 --- a/src/main_test.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/*************************************************** - This is an example for our Adafruit 16-channel PWM & Servo driver - Servo test - this will drive 8 servos, one after the other on the - first 8 pins of the PCA9685 - - Pick one up today in the adafruit shop! - ------> http://www.adafruit.com/products/815 - - These drivers use I2C to communicate, 2 pins are required to - interface. - - Adafruit invests time and resources providing this open source code, - please support Adafruit and open-source hardware by purchasing - products from Adafruit! - - Written by Limor Fried/Ladyada for Adafruit Industries. - BSD license, all text above must be included in any redistribution - ****************************************************/ - -#include -#include -#include - -// called this way, it uses the default address 0x40 -Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); -// you can also call it with a different address you want -//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41); -// you can also call it with a different address and I2C interface -//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire); - -// Depending on your servo make, the pulse width min and max may vary, you -// want these to be as small/large as possible without hitting the hard stop -// for max range. You'll have to tweak them as necessary to match the servos you -// have! -#define SERVOMIN 1000 // This is the 'minimum' pulse length count (out of 4096) -#define SERVOMAX 3095 // This is the 'maximum' pulse length count (out of 4096) -#define USMIN 4000 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 -#define USMAX 8000 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 -#define PWM_PERIOD 0.0025 // 2500 us -#define SERVO_FREQ 1/PWM_PERIOD -#define PWM_RES 12 -#define PWM_MAXDUTY (1 << PWM_RES) - 1 - -// our servo # counter -uint8_t servonum = 1; -void setup(){ - Serial.begin(115200); - Serial.println("8 channel Servo test!"); - - pwm.begin(); - /* - * In theory the internal oscillator (clock) is 25MHz but it really isn't - * that precise. You can 'calibrate' this by tweaking this number until - * you get the PWM update frequency you're expecting! - * The int.osc. for the PCA9685 chip is a range between about 23-27MHz and - * is used for calculating things like writeMicroseconds() - * Analog servos run at ~50 Hz updates, It is importaint to use an - * oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip. - * 1) Attach the oscilloscope to one of the PWM signal pins and ground on - * the I2C PCA9685 chip you are setting the value for. - * 2) Adjust setOscillatorFrequency() until the PWM update frequency is the - * expected value (50Hz for most ESCs) - * Setting the value here is specific to each individual I2C PCA9685 chip and - * affects the calculations for the PWM update frequency. - * Failure to correctly set the int.osc value will cause unexpected PWM results - */ - pwm.setOscillatorFrequency(27000000); - pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates - - delay(10); -} - -// You can use this function if you'd like to set the pulse length in seconds -// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise! -void setServoPulse(uint8_t n, double pulse) { - double pulselength; - - pulselength = 1000000; // 1,000,000 us per second - pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates - Serial.print(pulselength); Serial.println(" us per period"); - pulselength /= 4096; // 12 bits of resolution - Serial.print(pulselength); Serial.println(" us per bit"); - pulse *= 1000000; // convert input seconds to us - pulse /= pulselength; - Serial.println(pulse); - pwm.setPWM(n, 0, pulse); -} - -uint16_t power2Duty(float power) { - // this can be written in compiler code, but we are trying to save on flash memory - float tempTimeon = (power + 1) * 500 + 1000; - return tempTimeon / (PWM_PERIOD * 1000000) * (PWM_MAXDUTY); -} - -void loop() { - // Drive each servo one at a time using setPWM() - // Serial.println(servonum); - // for (uint16_t pulselen = SERVOMIN; pulselen < 2048; pulselen++) { - // pwm.setPWM(servonum, 0, pulselen); - // Serial.print("Pulse Length: "); - // Serial.println(pulselen); - // } - // delay(500); - // for (uint16_t pulselen = 2048; pulselen > SERVOMIN; pulselen--) { - // pwm.setPWM(servonum, 0, pulselen); - // Serial.print("Pulse Length: "); - // Serial.println(pulselen); - // } - - - // for(float i = -1; i < 1; i+=0.05){ - // Serial.print("i: "); - // Serial.print(i); - // Serial.print(" p2d: "); - // Serial.println(power2Duty(i)); - // pwm.setPWM(1,0,power2Duty(i)); - // delay(100); - // } - // for(float i = 1; i > -1; i-=0.05){ - // pwm.setPWM(1,0,power2Duty(i)); - // delay(100); - // } - - power2Duty(0.0f); - // delay(500); - -}