From 40e1d21e5e7221a280d94a75fb3ff6a9187b8a49 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 4 May 2022 21:13:44 +0200 Subject: [PATCH 1/8] change StepDirListener to use RISING interrupts --- src/communication/StepDirListener.cpp | 14 +++++++------- src/communication/StepDirListener.h | 4 +++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index c8f19b47..ee4f69e9 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -13,7 +13,7 @@ void StepDirListener::init(){ } void StepDirListener::enableInterrupt(void (*doA)()){ - attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE); + attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity); } void StepDirListener::attach(float* variable){ @@ -22,15 +22,15 @@ void StepDirListener::attach(float* variable){ void StepDirListener::handle(){ // read step status - bool step = digitalRead(pin_step); + //bool step = digitalRead(pin_step); // update counter only on rising edge - if(step && step != step_active){ - if(digitalRead(pin_dir)) + //if(step && step != step_active){ + if(digitalRead(pin_dir)) count++; - else + else count--; - } - step_active = step; + //} + //step_active = step; // if attached variable update it if(attached_variable) *attached_variable = getValue(); } diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 1c50c700..855a701d 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -47,11 +47,13 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing + PinStatus polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable float counter_to_value; //!< step counter to value - bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + }; #endif \ No newline at end of file From 9270663b2a4ab2337be3098fe7fcd92ece400184 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 25 May 2022 23:44:32 +0200 Subject: [PATCH 2/8] change polarity data type --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 855a701d..6047cf75 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -47,7 +47,7 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing - PinStatus polarity = RISING; //!< polarity of the step pin + int polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable From c87318caa2812050f6faafc8788432645cd2a208 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 24 Jul 2022 16:43:01 +0200 Subject: [PATCH 3/8] add debug to start/stop timers --- src/drivers/hardware_specific/stm32_mcu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index aedc4b3c..ee030395 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -167,6 +167,9 @@ void _startTimers(HardwareTimer **timers_to_start, int timer_num) for (int i=0; i < timer_num; i++) { if(timers_to_start[i] == NP) return; timers_to_start[i]->resume(); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + #endif } } From 581af181ae9083db8d7f8d27ae2736e15c7f007f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 19 Aug 2022 00:33:39 +0200 Subject: [PATCH 4/8] add ability to reverse PWM polarity on RP2040 --- src/drivers/hardware_specific/rp2040_mcu.cpp | 75 ++++++++++++-------- 1 file changed, 44 insertions(+), 31 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 2c127d71..15582b73 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -7,15 +7,28 @@ #define SIMPLEFOC_DEBUG_RP2040 -#ifdef SIMPLEFOC_DEBUG_RP2040 -#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL -#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial -#endif +#include "Arduino.h" +#include "communication/SimpleFOCDebug.h" -#endif -#include "Arduino.h" +// these defines determine the polarity of the PWM output. Normally, the polarity is active-high, +// i.e. a high-level PWM output is expected to switch on the MOSFET. But should your driver design +// require inverted polarity, you can change the defines below, or set them via your build environment +// or board definition files. + +// used for 2-PWM, 3-PWM, and 4-PWM modes +#ifndef SIMPLEFOC_PWM_ACTIVE_HIGH +#define SIMPLEFOC_PWM_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, high-side +#ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, low-side +#ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true +#endif @@ -48,16 +61,16 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 #ifdef SIMPLEFOC_DEBUG_RP2040 - SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue); + SimpleFOCDebug::print("Configuring pin "); + SimpleFOCDebug::print(pin); + SimpleFOCDebug::print(" slice "); + SimpleFOCDebug::print((int)slice); + SimpleFOCDebug::print(" channel "); + SimpleFOCDebug::print((int)chan); + SimpleFOCDebug::print(" frequency "); + SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" top value "); + SimpleFOCDebug::println(wrapvalue); #endif pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; @@ -84,8 +97,8 @@ void syncSlices() { void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); syncSlices(); return params; } @@ -95,9 +108,9 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); - setupPWM(pinC, pwm_frequency, false, params, 2); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pinC, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); syncSlices(); return params; } @@ -108,10 +121,10 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pin1A, pwm_frequency, false, params, 0); - setupPWM(pin1B, pwm_frequency, false, params, 1); - setupPWM(pin2A, pwm_frequency, false, params, 2); - setupPWM(pin2B, pwm_frequency, false, params, 3); + setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pin2A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); + setupPWM(pin2B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 3); syncSlices(); return params; } @@ -122,12 +135,12 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; params->dead_zone = dead_zone; - setupPWM(pinA_h, pwm_frequency, false, params, 0); - setupPWM(pinB_h, pwm_frequency, false, params, 2); - setupPWM(pinC_h, pwm_frequency, false, params, 4); - setupPWM(pinA_l, pwm_frequency, true, params, 1); - setupPWM(pinB_l, pwm_frequency, true, params, 3); - setupPWM(pinC_l, pwm_frequency, true, params, 5); + setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); + setupPWM(pinB_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 2); + setupPWM(pinC_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 4); + setupPWM(pinA_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 1); + setupPWM(pinB_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 3); + setupPWM(pinC_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 5); syncSlices(); return params; } From eb5347f4ad6f3f51642ae7fbd4cb1283fee6bafc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:03:55 +0200 Subject: [PATCH 5/8] added default PWM frequency to RP2040 driver --- src/drivers/hardware_specific/rp2040_mcu.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 15582b73..9fcd8e45 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -6,10 +6,7 @@ #define SIMPLEFOC_DEBUG_RP2040 - - -#include "Arduino.h" -#include "communication/SimpleFOCDebug.h" +#include "../hardware_api.h" // these defines determine the polarity of the PWM output. Normally, the polarity is active-high, @@ -31,6 +28,10 @@ #endif +#define _PWM_FREQUENCY 24000 +#define _PWM_FREQUENCY_MAX 66000 +#define _PWM_FREQUENCY_MIN 5000 + typedef struct RP2040DriverParams { int pins[6]; @@ -59,7 +60,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para pwm_set_phase_correct(slice, true); uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 - if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 + if (wrapvalue > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); SimpleFOCDebug::print(pin); @@ -96,6 +97,8 @@ void syncSlices() { void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -107,6 +110,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -120,6 +125,8 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -133,6 +140,8 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // non-PIO solution... RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; params->dead_zone = dead_zone; setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); From 14c27aab4c30eceba4ac03b9504c48d9acceb399 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:04:13 +0200 Subject: [PATCH 6/8] fix STM32 compile problem --- src/communication/StepDirListener.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 6047cf75..911ccbf4 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -4,6 +4,12 @@ #include "Arduino.h" #include "../common/foc_utils.h" + +#if defined(_STM32_DEF_) +#define PinStatus int +#endif + + /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -47,7 +53,7 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing - int polarity = RISING; //!< polarity of the step pin + PinStatus polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable From 17b460d6955da84a3881c3c69843e2be0b33a594 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:37:41 +0200 Subject: [PATCH 7/8] fix pinStatus issue --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 911ccbf4..da9a6a2d 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) #define PinStatus int #endif From c9e14b670f6c1cda7013d542680f03aadcf3b051 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:45:17 +0200 Subject: [PATCH 8/8] fix compile error for Due --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index da9a6a2d..7d7f2494 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) +#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE) #define PinStatus int #endif