FEAT added pwm frequency customisation

askuric · askuric · commit ff8784613d1d · 2020-10-04T13:07:03.000+02:00
diff --git a/README.md b/README.md
@@ -12,11 +12,17 @@ Therefore this is an attempt to:
 
 
 > ***SimpleFOClibrary* v1.6.0📢**<br>
-> - Teensy support by Christopher Parrott <br>
-> - Pull requests by [@cousinitt](https://github.com/cousinitt) - refactoring and c++11 improvements
+> - Teensy support by *Christopher Parrott* <br>
+> - Pull requests by [@cousinitt](https://github.com/cousinitt) 
+>   - refactoring and c++11 improvements
+>   - pid + low pass filter refactoring
 > - Extended configurability of the sensor classes [@owennewo](https://github.com/owennewo)
 > - **Stepper motor FOC support 🎨🎉 🎊**
 >   - Very easy to use and results are great, I love it!
+>   - short demo [youtube video](https://youtu.be/w_yIY0eXM5E)
+> - configurable pwm frequency
+>   - stm32,teensy,eps32 - not for  Arduino 
+>   - stm32 added 12bit pwm resolution by *Jürgen Frisch*
 > - Huge refactoring done in the library 😄
 > 
 >
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
@@ -21,7 +21,7 @@ BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en)
 
 
 // init hardware pins   
-void BLDCMotor::init() {
+void BLDCMotor::init(long pwm_frequency) {
   if(monitor_port) monitor_port->println("MOT: Init pins.");
   // PWM pins
   pinMode(pwmA, OUTPUT);
@@ -32,7 +32,7 @@ void BLDCMotor::init() {
   if(monitor_port) monitor_port->println("MOT: PWM config.");
   // Increase PWM frequency to 32 kHz
   // make silent
-  _setPwmFrequency(pwmA, pwmB, pwmC);
+  _setPwmFrequency(pwm_frequency, pwmA, pwmB, pwmC);
 
   // sanity check for the voltage limit configuration
   if(voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h
@@ -31,7 +31,7 @@ class BLDCMotor: public FOCMotor
     BLDCMotor(int phA,int phB,int phC,int pp, int en = NOT_SET);
     
     /**  Motor hardware init function */
-  	void init() override;
+  	void init(long pwm_frequency = NOT_SET) override;
     /** Motor disable function */
   	void disable() override;
     /** Motor enable function */
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
@@ -21,7 +21,7 @@ StepperMotor::StepperMotor(int ph1A, int ph1B, int ph2A, int ph2B, int pp, int e
 }
 
 // init hardware pins   
-void StepperMotor::init() {
+void StepperMotor::init(long pwm_frequency) {
   if(monitor_port) monitor_port->println("MOT: Init pins.");
   // PWM pins
   pinMode(pwm1A, OUTPUT);
@@ -34,7 +34,7 @@ void StepperMotor::init() {
   if(monitor_port) monitor_port->println("MOT: PWM config.");
   // Increase PWM frequency
   // make silent
-  _setPwmFrequency(pwm1A, pwm1B, pwm2A, pwm2B);
+  _setPwmFrequency(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
   
   // sanity check for the voltage limit configuration
   if(voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
diff --git a/src/StepperMotor.h b/src/StepperMotor.h
@@ -32,7 +32,7 @@ class StepperMotor: public FOCMotor
     StepperMotor(int ph1A,int ph1B,int ph2A,int ph2B,int pp, int en1 = NOT_SET, int en2 = NOT_SET);
     
     /**  Motor hardware init function */
-  	void init() override;
+  	void init(long pwm_frequency = NOT_SET) override;
     /** Motor disable function */
   	void disable() override;
     /** Motor enable function */
diff --git a/src/common/FOCMotor.h b/src/common/FOCMotor.h
@@ -42,7 +42,7 @@ class FOCMotor
     FOCMotor();
 
     /**  Motor hardware init function */
-  	virtual void init()=0;
+  	virtual void init(long pwm_frequency)=0;
     /** Motor disable function */
   	virtual void disable()=0;
     /** Motor enable function */
diff --git a/src/common/hardware_utils.cpp b/src/common/hardware_utils.cpp
@@ -14,122 +14,176 @@ typedef struct {
   mcpwm_io_signals_t mcpwm_a;
   mcpwm_io_signals_t mcpwm_b;
   mcpwm_io_signals_t mcpwm_c;
+} bldc_motor_slots_t;
+typedef struct {
+  int pin1A;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  mcpwm_io_signals_t mcpwm_1a;
+  mcpwm_io_signals_t mcpwm_1b;
+  mcpwm_io_signals_t mcpwm_2a;
+  mcpwm_io_signals_t mcpwm_2b;
+} stepper_motor_slots_t;
 
-} motor_slots_t;
-
-// define motor slots array
-motor_slots_t esp32_motor_slots[4] =  { 
+// define bldc motor slots array
+bldc_motor_slots_t esp32_bldc_motor_slots[4] =  { 
   {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A
   {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B
   {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A
   {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}  // 4th motor will be MCPWM1 channel B
   };
 
-#endif
+// define stepper motor slots array
+stepper_motor_slots_t esp32_stepper_motor_slots[2] =  { 
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0
+  };  
 
+// configuring high frequency pwm timer
+// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller
+void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num,  mcpwm_unit_t mcpwm_unit){
 
-// function setting the high pwm frequency to the supplied pins
-// - hardware speciffic
-// supports Arudino/ATmega328, STM32 and ESP32 
-void _setPwmFrequency(const int pinA, const int pinB, const int pinC, const int pinD) {
-#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips
-   //  High PWM frequency
+  mcpwm_config_t pwm_config;
+  pwm_config.frequency = pwm_frequency;  //frequency
+  pwm_config.cmpr_a = 0;      //duty cycle of PWMxA = 50.0%
+  pwm_config.cmpr_b = 0;      //duty cycle of PWMxB = 50.0%
+  pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
+  pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+
+  _delay(100);
+
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
+  mcpwm_num->clk_cfg.prescale = 0;
+
+  mcpwm_num->timer[0].period.prescale = 4;
+  mcpwm_num->timer[1].period.prescale = 4;
+  mcpwm_num->timer[2].period.prescale = 4;    
+  _delay(1);
+  mcpwm_num->timer[0].period.period = 2048;
+  mcpwm_num->timer[1].period.period = 2048;
+  mcpwm_num->timer[2].period.period = 2048;
+  _delay(1);
+  mcpwm_num->timer[0].period.upmethod = 0;
+  mcpwm_num->timer[1].period.upmethod = 0;
+  mcpwm_num->timer[2].period.upmethod = 0;
+  _delay(1); 
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
+
+  mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0);
+  mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0);
+  mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0);
+  _delay(1);
+  mcpwm_num->timer[0].sync.out_sel = 1;
+  _delay(1);
+  mcpwm_num->timer[0].sync.out_sel = 0;
+}
+
+#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin){
+  //  High PWM frequency
   //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
-  if (pinA == 5 || pinA == 6 || pinB == 5 || pinB == 6 || pinC == 5 || pinC == 6 || pinD == 5 || pinD == 6 ) {
+   if (pin == 5 || pin == 6  ) {
       TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
       TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
   }
-  if (pinA == 9 || pinA == 10 || pinB == 9 || pinB == 10 || pinC == 9 || pinC == 10|| pinD == 9 || pinD == 10 )
+  if (pin == 9 || pin == 10 )
       TCCR1B = ((TCCR1B & 0b11111000) | 0x01);     // set prescaler to 1
-  if (pinA == 3 || pinA == 11 || pinB == 3 || pinB == 11 || pinC == 3 || pinC == 11 || pinD == 3 || pinD == 11 ) 
+  if (pin == 3 || pin == 11) 
       TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1
   
+}
 #elif defined(_STM32_DEF_) // if stm chips
-
-  analogWrite(pinA, 0);
-  analogWriteFrequency(50000);  // set 50kHz
-  analogWriteResolution(12); // resolution 12 bit 0 - 4096
-  analogWrite(pinB, 0);
-  analogWriteFrequency(50000);  // set 50kHz
-  analogWriteResolution(12); // resolution 12 bit 0 - 4096
-  analogWrite(pinC, 0);
-  analogWriteFrequency(50000);  // set 50kHz
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFrequency(freq); 
   analogWriteResolution(12); // resolution 12 bit 0 - 4096
-  if(pinD) {
-    analogWrite(pinD, 0);
-    analogWriteFrequency(50000);  // set 50kHz
-    analogWriteResolution(12); // resolution 12 bit 0 - 4096
-  }
+}
 #elif defined(__arm__) && defined(CORE_TEENSY) //if teensy 3x / 4x / LC boards
-  analogWrite(pinA, 0);
-  analogWriteFrequency(pinA, 50000);  // set 50kHz
-  analogWrite(pinB, 0);
-  analogWriteFrequency(pinB, 50000);  // set 50kHz
-  analogWrite(pinC, 0);
-  analogWriteFrequency(pinC, 50000);  // set 50kHz
-  if(pinD) {
-    analogWrite(pinD, 0);
-    analogWriteFrequency(50000);  // set 50kHz
-  }
-#elif defined(ESP_H) // if esp32 boards
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFrequency(freq); 
+}
+#endif
 
-  motor_slots_t m_slot = {};
 
-  // determine which motor are we connecting
-  // and set the appropriate configuration parameters 
-  for(int i = 0; i < 4; i++){
-    if(esp32_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
-      esp32_motor_slots[i].pinA = pinA;
-      m_slot = esp32_motor_slots[i];
-      break;
-    }
-  }
-        
-  // configure pins
-  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
-  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
-  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
+// function setting the high pwm frequency to the supplied pins
+// - hardware speciffic
+// supports Arudino/ATmega328, STM32 and ESP32 
+void _setPwmFrequency(const long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  _pinHighFrequency(pinC);
+  if(pinD != NOT_SET) _pinHighFrequency(pinD); // stepper motor
 
-  mcpwm_config_t pwm_config;
-  pwm_config.frequency = 4000000;  //frequency = 20000Hz
-  pwm_config.cmpr_a = 0;      //duty cycle of PWMxA = 50.0%
-  pwm_config.cmpr_b = 0;      //duty cycle of PWMxB = 50.0%
-  pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
-  pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
-  mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_0, &pwm_config);    //Configure PWM0A & PWM0B with above settings
-  mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_1, &pwm_config);    //Configure PWM0A & PWM0B with above settings
-  mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_2, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+#elif defined(_STM32_DEF_) || (defined(__arm__) && defined(CORE_TEENSY)) //if stm32 or  teensy 3x / 4x / LC boards
+  if(pwm_frequency == NOT_SET) pwm_frequency = 500000; // default frequency 50khz
+  else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor
 
-  _delay(100);
+#elif defined(ESP_H) // if esp32 boards
+  if(pwm_frequency == NOT_SET) pwm_frequency = 400000; // default frequency 40khz
+  else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
 
-  mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_0);
-  mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_1);
-  mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_2);
-  m_slot.mcpwm_num->clk_cfg.prescale = 0;
+  if(pinD == NOT_SET){
+    bldc_motor_slots_t m_slot = {};
 
-  m_slot.mcpwm_num->timer[0].period.prescale = 4;
-  m_slot.mcpwm_num->timer[1].period.prescale = 4;
-  m_slot.mcpwm_num->timer[2].period.prescale = 4;    
-  _delay(1);
-  m_slot.mcpwm_num->timer[0].period.period = 2048;
-  m_slot.mcpwm_num->timer[1].period.period = 2048;
-  m_slot.mcpwm_num->timer[2].period.period = 2048;
-  _delay(1);
-  m_slot.mcpwm_num->timer[0].period.upmethod = 0;
-  m_slot.mcpwm_num->timer[1].period.upmethod = 0;
-  m_slot.mcpwm_num->timer[2].period.upmethod = 0;
-  _delay(1); 
-  mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_0);
-  mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_1);
-  mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_2);
+    // determine which motor are we connecting
+    // and set the appropriate configuration parameters 
+    for(int i = 0; i < 4; i++){
+      if(esp32_bldc_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
+        esp32_bldc_motor_slots[i].pinA = pinA;
+        m_slot = esp32_bldc_motor_slots[i];
+        break;
+      }
+    }
+          
+    // configure pins
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
 
-  mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0);
-  mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0);
-  mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0);
-  _delay(1);
-  m_slot.mcpwm_num->timer[0].sync.out_sel = 1;
-  _delay(1);
-  m_slot.mcpwm_num->timer[0].sync.out_sel = 0;
+    // configure the timer
+    _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+  }else{
+    stepper_motor_slots_t m_slot = {};
+    // determine which motor are we connecting
+    // and set the appropriate configuration parameters 
+    for(int i = 0; i < 2; i++){
+      if(esp32_stepper_motor_slots[i].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
+        esp32_stepper_motor_slots[i].pin1A = pinA;
+        m_slot = esp32_stepper_motor_slots[i];
+        break;
+      }
+    }
+          
+    // configure pins
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA);
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB);
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC);
+    mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD);
+
+    // configure the timer
+    _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+  }
 #endif
 }
 
@@ -142,12 +196,12 @@ void _writeDutyCycle(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, in
 #if defined(ESP_H) // if ESP32 boards
   // determine which motor slot is the motor connected to 
   for(int i = 0; i < 4; i++){
-    if(esp32_motor_slots[i].pinA == pinA){ // if motor slot found
+    if(esp32_bldc_motor_slots[i].pinA == pinA){ // if motor slot found
       // se the PWM on the slot timers
       // transform duty cycle from [0,1] to [0,2047]
-      esp32_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047;
-      esp32_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047;
-      esp32_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047;
+      esp32_bldc_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047;
+      esp32_bldc_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047;
+      esp32_bldc_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047;
       break;
     }
   }
@@ -171,7 +225,18 @@ void _writeDutyCycle(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, in
 // ESP32 uses MCPWM
 void _writeDutyCycle(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
 #if defined(ESP_H) // if ESP32 boards
-  // not sure how to handle this
+  // determine which motor slot is the motor connected to 
+  for(int i = 0; i < 2; i++){
+    if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,2047]
+      esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1a*2047;
+      esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1b*2047;
+      esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2a*2047;
+      esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2b*2047;
+      break;
+    }
+  }
 #elif defined(_STM32_DEF_) // STM32 devices
   // transform duty cycle from [0,1] to [0,4095]
   analogWrite(pin1A, 4095.0*dc_1a);
diff --git a/src/common/hardware_utils.h b/src/common/hardware_utils.h
