From c857bd89079e82778679633abe5679b575e5c4b8 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 21 Mar 2023 19:11:31 +0100 Subject: [PATCH 001/220] make foc_utils functions WEAK --- src/common/foc_utils.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 11a1810f..27194c8a 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,17 +1,18 @@ #include "foc_utils.h" -// int array instead of float array -// 4x200 points per 360 deg -// 2x storage save (int 2Byte float 4 Byte ) -// sin*10000 -const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; // function approximating the sine calculation by using fixed size array // ~40us (float array) // ~50us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -float _sin(float a){ +WEAK float _sin(float a){ + // int array instead of float array + // 4x200 points per 360 deg + // 2x storage save (int 2Byte float 4 Byte ) + // sin*10000 + static const uint16_t sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; + if(a < _PI_2){ //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; //return sine_array[(int)(126.6873f* a)]; // float array optimized @@ -36,7 +37,7 @@ float _sin(float a){ // ~56us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -float _cos(float a){ +WEAK float _cos(float a){ float a_sin = a + _PI_2; a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; return _sin(a_sin); @@ -44,7 +45,7 @@ float _cos(float a){ // normalizing radian angle to [0,2PI] -float _normalizeAngle(float angle){ +WEAK float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } @@ -57,15 +58,13 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root -float _sqrtApprox(float number) {//low in fat - long i; - float y; +WEAK float _sqrtApprox(float number) {//low in fat // float x; // const float f = 1.5F; // better precision // x = number * 0.5F; - y = number; - i = * ( long * ) &y; + float y = number; + long i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision From d752f291b71905666283c2fafda59f3f33760db3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 21 Mar 2023 19:54:37 +0100 Subject: [PATCH 002/220] seems the WEAK macro isn't declared on all archs --- src/common/foc_utils.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 27194c8a..c38fd897 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -6,7 +6,7 @@ // ~50us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -WEAK float _sin(float a){ +__attribute__((weak)) float _sin(float a){ // int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) @@ -37,7 +37,7 @@ WEAK float _sin(float a){ // ~56us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -WEAK float _cos(float a){ +__attribute__((weak)) float _cos(float a){ float a_sin = a + _PI_2; a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; return _sin(a_sin); @@ -45,7 +45,7 @@ WEAK float _cos(float a){ // normalizing radian angle to [0,2PI] -WEAK float _normalizeAngle(float angle){ +__attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } @@ -58,7 +58,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root -WEAK float _sqrtApprox(float number) {//low in fat +__attribute__((weak)) float _sqrtApprox(float number) {//low in fat // float x; // const float f = 1.5F; // better precision From 1010207f3094859eca4688e581bf8549987715e0 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 21 Mar 2023 22:22:51 +0100 Subject: [PATCH 003/220] fix RP2040 ADCEngine stop - #259 --- src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 7bcd23aa..40c13e34 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -241,6 +241,7 @@ void RP2040ADCEngine::start() { void RP2040ADCEngine::stop() { adc_run(false); + irq_set_enabled(DMA_IRQ_0, false); dma_channel_abort(readDMAChannel); // if (triggerPWMSlice>=0) // dma_channel_abort(triggerDMAChannel); From fddf80c9b499eb78ec4a0f4bcb5d230b70db2a7c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 30 Mar 2023 10:01:11 +0200 Subject: [PATCH 004/220] add overflow check to STM32 HW dead-time calc --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index d16808f6..211f7f37 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -287,6 +287,7 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* // dead time is set in nanoseconds uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + if (dead_time>255) dead_time = 255; LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); From 5e274ad4c33bff839a128f238aa93e64f5466d38 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 30 Mar 2023 10:30:39 +0200 Subject: [PATCH 005/220] add check for dead-time overflow on STM32 --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 211f7f37..acf6d22b 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -288,6 +288,10 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); if (dead_time>255) dead_time = 255; + if (dead_time==0 && dead_zone>0) { + dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large + SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max"); + } LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); From df5f6de6189b66f557a6b7ee7fe60a7515ff44e8 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 31 Mar 2023 11:29:35 +0200 Subject: [PATCH 006/220] inital implementation of the velocity feed forward --- src/BLDCMotor.cpp | 3 ++- src/StepperMotor.cpp | 3 ++- src/common/base_classes/FOCMotor.h | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fce552c6..499ac4d3 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -418,7 +418,8 @@ void BLDCMotor::move(float new_target) { // angle set point shaft_angle_sp = target; // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit); // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8865f57c..48d2cc27 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -307,7 +307,8 @@ void StepperMotor::move(float new_target) { // angle set point shaft_angle_sp = target; // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c499df4a..dc31f3a9 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -155,6 +155,7 @@ class FOCMotor // state variables float target; //!< current target value - depends of the controller + float feed_forward_velocity = 0.0f; //!< current feed forward velocity float shaft_angle;//!< current motor angle float electrical_angle;//!< current electrical angle float shaft_velocity;//!< current motor velocity From b12be53ad067fb4351f4725d8df1a395bb1e5b17 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 12 May 2023 23:01:43 +0200 Subject: [PATCH 007/220] fix micros overflow error in Sensor.cpp --- src/common/base_classes/Sensor.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 28511762..a80fa438 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -18,7 +18,12 @@ void Sensor::update() { float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; - // TODO handle overflow - we do need to reset vel_angle_prev_ts + if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; + return velocity; + } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; From 3bb05d0371637284b92387d699be3fb0a07901fe Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 12 May 2023 23:01:59 +0200 Subject: [PATCH 008/220] Simplify HW-dependent define --- 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 9ae13362..44b0e531 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) || defined(ARDUINO_SAM_DUE) || defined(CORE_TEENSY) || defined(NRF52_SERIES) +#if !defined(TARGET_RP2040) #define PinStatus int #endif From 9b68c8e12022b4510e8360ec85a063190c7bdb03 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 12 May 2023 23:02:57 +0200 Subject: [PATCH 009/220] fix [BUG] RP2040 ADC Engine #264 --- src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 40c13e34..22bb891a 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -10,6 +10,7 @@ #include "hardware/dma.h" #include "hardware/irq.h" #include "hardware/pwm.h" +#include "hardware/adc.h" /* Singleton instance of the ADC engine */ From 95bb2c3fde21c76cce53fe5980793dd4828997c6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 12 May 2023 23:11:17 +0200 Subject: [PATCH 010/220] fix SAMD compile error --- 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 44b0e531..5de0783f 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if !defined(TARGET_RP2040) +#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) #define PinStatus int #endif From 55debc4acb0c945d6967a2451d5615041c5b1a7d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 12 May 2023 23:38:09 +0200 Subject: [PATCH 011/220] change NOT_SET to float type --- src/common/foc_utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e1e0b153..c5ff1852 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -28,7 +28,7 @@ #define _PI_6 0.52359877559f #define _RPM_TO_RADS 0.10471975512f -#define NOT_SET -12345.0 +#define NOT_SET -12345.0f #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 From 5e35fb8d0d330cab75c40994012aac307d9c4636 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 01:24:37 -0500 Subject: [PATCH 012/220] Fix for issue #270 Overhaul of interrupt-based sensors to eliminate bad readings due to interrupts modifying variables in the middle of get functions. All sensor classes are now required to use the base class state variables, and do their angle reading once per frame in the update() function rather than using the virtual get functions to return real-time updated readings. I have chosen to make an exception for getVelocity() and allow it to access volatile variables as well, although it may be better to merge it into update() instead. Interrupts should be disabled/enabled as necessary in the update() and getVelocity() functions, and preferably nowhere else. The Arduino library provides no mechanism to restore the previous state of interrupts, so use of the unconditional enable should be kept to as few locations as possible so we can be reasonably sure that it won't be called at a time when interrupts should remain disabled. Additional changes to specific sensors: HallSensor: Removed the max_velocity variable because it was a quick fix for bad velocity readings that were coming from this interrupt problem, which should no longer occur. Also changed the condition for "velocity too old" from (_micros() - pulse_timestamp) > pulse_diff to (_micros() - pulse_timestamp) > pulse_diff*2, because any deceleration would cause inappropriate reporting of zero velocity. MagneticSensorPWM: Unrelated to the interrupt problem, timestamp is now saved from the rising edge of the PWM pulse because that's when the angle was sampled, and communicating it takes a significant and variable amount of time. This gives more accurate velocity calculations, and will allow extrapolating accurate angles using the new SmoothingSensor class. --- src/common/base_classes/Sensor.h | 1 + src/sensors/Encoder.cpp | 28 ++++++++---------- src/sensors/HallSensor.cpp | 47 ++++++++++--------------------- src/sensors/HallSensor.h | 6 ++-- src/sensors/MagneticSensorPWM.cpp | 17 ++++++++++- src/sensors/MagneticSensorPWM.h | 4 +++ 6 files changed, 50 insertions(+), 53 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index b8834e8e..c77eb911 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -42,6 +42,7 @@ enum Pullup : uint8_t { * */ class Sensor{ + friend class SmoothingSensor; public: /** * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 96057d82..2e4a70da 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -99,29 +99,21 @@ void Encoder::handleIndex() { } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { - // do nothing for Encoder + noInterrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = pulse_counter / (int)cpr; + angle_prev = _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); + angle_prev_ts = pulse_timestamp; + interrupts(); } /* Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getAngle(); -} -// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost -float Encoder::getMechanicalAngle(){ - return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); -} - -float Encoder::getAngle(){ - return _2PI * (pulse_counter) / ((float)cpr); -} -double Encoder::getPreciseAngle(){ - return _2PI * (pulse_counter) / ((double)cpr); -} -int32_t Encoder::getFullRotations(){ - return pulse_counter / (int)cpr; + return _2PI * (pulse_counter) / ((float)cpr); } @@ -131,6 +123,8 @@ int32_t Encoder::getFullRotations(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ + // Make sure no interrupts modify the state variables in the middle of these calculations + noInterrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -162,6 +156,8 @@ float Encoder::getVelocity(){ // save velocity calculation variables prev_Th = Th; prev_pulse_counter = pulse_counter; + // Re-enable interrupts (ideally this would restore to the previous state rather than unconditionally enabling) + interrupts(); return velocity; } diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 7d1d9f64..32ed54e2 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -94,8 +94,13 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { -float HallSensor::getSensorAngle() { - return getAngle(); +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void HallSensor::update() { + noInterrupts(); + angle_prev = ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((electric_rotations * 6 + electric_sector) / cpr); + angle_prev_ts = pulse_timestamp; + interrupts(); } @@ -104,8 +109,8 @@ float HallSensor::getSensorAngle() { Shaft angle calculation TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getMechanicalAngle() { - return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; +float HallSensor::getSensorAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } /* @@ -113,38 +118,16 @@ float HallSensor::getMechanicalAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - long last_pulse_diff = pulse_diff; - if (last_pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > last_pulse_diff) ) { // last velocity isn't accurate if too old - return 0; - } else { - float vel = direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); - // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 - if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero - return vel; - } - + noInterrupts(); + float vel = 0; + if (pulse_diff != 0 && ((long)(_micros() - pulse_timestamp) < pulse_diff*2) ) // last velocity isn't accurate if too old + vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + interrupts(); + return vel; } -float HallSensor::getAngle() { - return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; -} - - -double HallSensor::getPreciseAngle() { - return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; -} - - -int32_t HallSensor::getFullRotations() { - return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); -} - - - - - // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index d6b4493a..ad50c7d5 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -53,14 +53,12 @@ class HallSensor: public Sensor{ int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation + /** Interrupt-safe update */ + void update() override; /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 02798ad3..3b0cd9aa 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -56,10 +56,21 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + pulse_timestamp = _micros(); this->Sensor::init(); // call base class init } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void MagneticSensorPWM::update() { + if (is_interrupt_based) + noInterrupts(); + Sensor::update(); + angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication + if (is_interrupt_based) + interrupts(); +} + // get current angle (rad) float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor @@ -73,6 +84,7 @@ float MagneticSensorPWM::getSensorAngle(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way + pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse pulse_length_us = pulseIn(pinPWM, HIGH); } return pulse_length_us; @@ -84,7 +96,10 @@ void MagneticSensorPWM::handlePWM() { unsigned long now_us = _micros(); // if falling edge, calculate the pulse length - if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; + if (!digitalRead(pinPWM)) { + pulse_length_us = now_us - last_call_us; + pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp + } // save the currrent timestamp for the next call last_call_us = now_us; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 77e82459..48492c84 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -32,6 +32,9 @@ class MagneticSensorPWM: public Sensor{ void init(); int pinPWM; + + // Interrupt-safe update + void update() override; // get current angle (rad) float getSensorAngle() override; @@ -62,6 +65,7 @@ class MagneticSensorPWM: public Sensor{ // time tracking variables unsigned long last_call_us; // unsigned long pulse_length_us; + unsigned long pulse_timestamp; }; From 98a10e4e96e0473784997f4ebfbac8d3d3c780da Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 03:48:40 -0500 Subject: [PATCH 013/220] Reduced interrupt blocking time Changed interrupt-based sensors to only copy volatile variables during interrupt-blocking sections, and do computations with them after re-enabling interrupts. --- src/sensors/Encoder.cpp | 21 ++++++++++++--------- src/sensors/HallSensor.cpp | 21 +++++++++++++-------- 2 files changed, 25 insertions(+), 17 deletions(-) diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 2e4a70da..fa4b8e73 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -101,12 +101,14 @@ void Encoder::handleIndex() { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); - // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost - full_rotations = pulse_counter / (int)cpr; - angle_prev = _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); angle_prev_ts = pulse_timestamp; + long copy_pulse_counter = pulse_counter; interrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = copy_pulse_counter / (int)cpr; + angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); } /* @@ -123,8 +125,11 @@ float Encoder::getSensorAngle(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ - // Make sure no interrupts modify the state variables in the middle of these calculations + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); + long copy_pulse_counter = pulse_counter; + long copy_pulse_timestamp = pulse_timestamp; + interrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -133,8 +138,8 @@ float Encoder::getVelocity(){ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6f; - long dN = pulse_counter - prev_pulse_counter; + float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f; + long dN = copy_pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) // dN - impulses received @@ -155,9 +160,7 @@ float Encoder::getVelocity(){ prev_timestamp_us = timestamp_us; // save velocity calculation variables prev_Th = Th; - prev_pulse_counter = pulse_counter; - // Re-enable interrupts (ideally this would restore to the previous state rather than unconditionally enabling) - interrupts(); + prev_pulse_counter = copy_pulse_counter; return velocity; } diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 32ed54e2..38767d59 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,11 +96,14 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void HallSensor::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); - angle_prev = ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; - full_rotations = (int32_t)((electric_rotations * 6 + electric_sector) / cpr); angle_prev_ts = pulse_timestamp; + long last_electric_rotations = electric_rotations; + int8_t last_electric_sector = electric_sector; interrupts(); + angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -119,14 +122,16 @@ float HallSensor::getSensorAngle() { */ float HallSensor::getVelocity(){ noInterrupts(); - float vel = 0; - if (pulse_diff != 0 && ((long)(_micros() - pulse_timestamp) < pulse_diff*2) ) // last velocity isn't accurate if too old - vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + long last_pulse_timestamp = pulse_timestamp; + long last_pulse_diff = pulse_diff; interrupts(); - return vel; -} - + if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old + return 0; + } else { + return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); + } +} // HallSensor initialisation of the hardware pins // and calculation variables From b03f8a91dfc78cedbaabcf88cc70e6f65d064165 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 04:41:17 -0500 Subject: [PATCH 014/220] Forgot to remove function headers --- src/sensors/Encoder.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 91427a83..af6a5ab6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,12 +61,8 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getAngle() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; virtual void update() override; /** From cbc0be105b556228e0c62d08bf36aa27deb3344e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 24 May 2023 09:57:17 +0200 Subject: [PATCH 015/220] BLDCMotor typo angle instead of velocity --- src/BLDCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 499ac4d3..09567aa7 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -419,7 +419,7 @@ void BLDCMotor::move(float new_target) { shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); - shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit); + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage From 1df947091ba4993e90c249cd75e856d20241114c Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 24 May 2023 09:58:25 +0200 Subject: [PATCH 016/220] StepperMotor typo, angle instead velocity --- src/StepperMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 48d2cc27..aed56807 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -308,7 +308,7 @@ void StepperMotor::move(float new_target) { shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); - shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit); + shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage @@ -440,4 +440,4 @@ float StepperMotor::angleOpenloop(float target_angle){ open_loop_timestamp = now_us; return Uq; -} \ No newline at end of file +} From 99b3f9d8577b47058ee573d7177b8a4a817d6ef8 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 25 May 2023 10:37:18 +0200 Subject: [PATCH 017/220] teensy 3 and 4 bugfix for 6pwm, forgotten PhaseState parameter --- src/drivers/hardware_specific/teensy/teensy3_mcu.cpp | 3 ++- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp index 742d79df..de48dad0 100644 --- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -206,7 +206,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - Stepper motor - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); // transform duty cycle from [0,1] to [0,255] // phase A analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index c961ad06..9ed33443 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -290,7 +290,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - Stepper motor - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); From ae5accbb253c48053788c8ada65f5da17e484d99 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 30 May 2023 22:52:56 +0200 Subject: [PATCH 018/220] extend RP2040 PWM to support low speeds --- .../hardware_specific/rp2040/rp2040_mcu.cpp | 19 ++++++++++++++----- .../hardware_specific/rp2040/rp2040_mcu.h | 2 ++ 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index a2970c72..0cc333ad 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -30,11 +30,12 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para params->pins[index] = pin; params->slice[index] = slice; params->chan[index] = chan; - pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get - 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 > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 + uint32_t sysclock_hz = machine.freq(); + uint32_t factor = 4096 * 2 * pwm_frequency; + uint32_t div = sysclock_hz / factor; + if (sysclock_hz % factor !=0) div+=1; + if (div < 16) div = 16; + uint32_t wrapvalue = sysclock_hz *16 / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); SimpleFOCDebug::print(pin); @@ -44,9 +45,17 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para SimpleFOCDebug::print((int)chan); SimpleFOCDebug::print(" frequency "); SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" divisor "); + SimpleFOCDebug::print((int)(div>>4)); + SimpleFOCDebug::print("."); + SimpleFOCDebug::print((int)(div&0xF)); SimpleFOCDebug::print(" top value "); SimpleFOCDebug::println(wrapvalue); #endif + if (wrapvalue < 999) + SimpleFOCDebug::println("Warning: PWM resolution is low."); + pwm_set_clkdiv_int_frac(slice, div>>4, div&0xF); + pwm_set_phase_correct(slice, true); pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; if (invert) { diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h index c3b4fe51..bbfb3873 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -2,6 +2,8 @@ #pragma once +#include "Arduino.h" + #if defined(TARGET_RP2040) From 3cee9cfb209a7d988f21130a8d00f180f5725eeb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 1 Jun 2023 23:42:04 +0200 Subject: [PATCH 019/220] make isSentinel useable for other code --- src/communication/Commander.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 3633a510..91e3dc45 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -240,6 +240,7 @@ class Commander */ void motion(FOCMotor* motor, char* user_cmd, char* separator = (char *)" "); + bool isSentinel(char ch); private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number @@ -294,7 +295,6 @@ class Commander void printError(); - bool isSentinel(char ch); }; From fa637deed3fb32d6a15916394429d4b4dea29138 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 1 Jun 2023 23:58:18 +0200 Subject: [PATCH 020/220] RP2040 PWM driver now supports pre-scaler --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index 0cc333ad..ebe1a7dd 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -9,10 +9,11 @@ #include "../../hardware_api.h" #include "./rp2040_mcu.h" #include "hardware/pwm.h" +#include "hardware/clocks.h" #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 -#define _PWM_FREQUENCY_MIN 5000 +#define _PWM_FREQUENCY_MIN 1 @@ -30,12 +31,12 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para params->pins[index] = pin; params->slice[index] = slice; params->chan[index] = chan; - uint32_t sysclock_hz = machine.freq(); + uint32_t sysclock_hz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS) * 1000; uint32_t factor = 4096 * 2 * pwm_frequency; uint32_t div = sysclock_hz / factor; if (sysclock_hz % factor !=0) div+=1; if (div < 16) div = 16; - uint32_t wrapvalue = sysclock_hz *16 / div / pwm_frequency - 1; + uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); SimpleFOCDebug::print(pin); @@ -50,7 +51,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para SimpleFOCDebug::print("."); SimpleFOCDebug::print((int)(div&0xF)); SimpleFOCDebug::print(" top value "); - SimpleFOCDebug::println(wrapvalue); + SimpleFOCDebug::println((int)wrapvalue); #endif if (wrapvalue < 999) SimpleFOCDebug::println("Warning: PWM resolution is low."); From 505248c2c2052567d28df84cc36eff4a81b7bdb1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 19 Jun 2023 00:24:53 +0200 Subject: [PATCH 021/220] move default sample rate to header --- src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp | 1 - src/current_sense/hardware_specific/rp2040/rp2040_mcu.h | 5 ++++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 22bb891a..613c6323 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -164,7 +164,6 @@ bool RP2040ADCEngine::init() { false, // We won't see the ERR bit because of 8 bit reads; disable. true // Shift each sample to 8 bits when pushing to FIFO ); - samples_per_second = 20000; if (samples_per_second<1 || samples_per_second>=500000) { samples_per_second = 0; adc_set_clkdiv(0); diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h index b673fc30..ae28bb26 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h @@ -22,6 +22,9 @@ * Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz. * After starting the engine it will continuously sample and provide new values at the configured rate. * + * The default sampling rate is 20kHz, which is suitable for 2 channels assuming you a 5kHz main loop speed (a new measurement is used per + * main loop iteration). + * * Low-side sensing is currently not supported. * * The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current @@ -74,7 +77,7 @@ class RP2040ADCEngine { ADCResults getLastResults(); // TODO find a better API and representation for this - int samples_per_second = 0; // leave at 0 to convert in tight loop + int samples_per_second = 20000; // 20kHz default (assuming 2 shunts and 5kHz loop speed), set to 0 to convert in tight loop float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float //int triggerPWMSlice = -1; From 5fc3128d282b65c141ca486327c6235089999627 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 12:27:26 +0200 Subject: [PATCH 022/220] fix KV initialization bug --- src/BLDCMotor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 499ac4d3..cfab925d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -44,7 +44,9 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/KV // 1/sqrt(2) - rms value - KV_rating = _KV*_SQRT2; + KV_rating = NOT_SET; + if (_isset(_KV)) + KV_rating = _KV*_SQRT2; // save phase inductance phase_inductance = _inductance; From 1496e51122695704333d570fa43b19848ee6faea Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 21:01:41 +0200 Subject: [PATCH 023/220] make Commander work with \r also --- src/communication/Commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 7aa8865f..ee6be9f9 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -593,6 +593,7 @@ bool Commander::isSentinel(char ch) else if (ch == '\r') { printVerbose(F("Warn: \\r detected! \n")); + return true; // lets still consider it to end the line... } return false; } From d57d32dd8715dbed4e476469bc3de0c052f1d531 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 21:46:41 +0200 Subject: [PATCH 024/220] change sensor_direction from int to Direction --- src/BLDCMotor.cpp | 2 +- src/common/base_classes/FOCMotor.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index cfab925d..cec5efb4 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -213,7 +213,7 @@ int BLDCMotor::alignSensor() { if(!exit_flag) return exit_flag; // if unknown natural direction - if(!_isset(sensor_direction)){ + if(sensor_direction==Direction::UNKNOWN){ // find natural direction // move one electrical revolution forward diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index dc31f3a9..748674cf 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -109,7 +109,7 @@ class FOCMotor * @param sensor_direction sensor natural direction - default is CW * */ - virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + virtual int initFOC(float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -209,7 +209,7 @@ class FOCMotor // sensor related variabels float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available - int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW + Direction sensor_direction = Direction::UNKNOWN; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW /** * Function providing BLDCMotor class with the From 5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 22:04:26 +0200 Subject: [PATCH 025/220] change init logic - remove params from initFOC --- src/BLDCMotor.cpp | 9 +-------- src/BLDCMotor.h | 2 +- src/StepperMotor.cpp | 9 +-------- src/StepperMotor.h | 6 +----- src/common/base_classes/FOCMotor.h | 8 ++------ 5 files changed, 6 insertions(+), 28 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index cec5efb4..45108377 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -126,20 +126,13 @@ void BLDCMotor::enable() FOC functions */ // FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { +int BLDCMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 366f4600..a1f196ab 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -49,7 +49,7 @@ class BLDCMotor: public FOCMotor * Function initializing FOC algorithm * and aligning sensor's and motors' zero position */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 48d2cc27..f1e51cce 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -92,20 +92,13 @@ void StepperMotor::enable() FOC functions */ // FOC initialization function -int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { +int StepperMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 36f4fe3f..7eda3167 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -54,12 +54,8 @@ class StepperMotor: public FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 748674cf..56e7a516 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -104,12 +104,8 @@ class FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - virtual int initFOC(float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + virtual int initFOC()=0; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -209,7 +205,7 @@ class FOCMotor // sensor related variabels float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available - Direction sensor_direction = Direction::UNKNOWN; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW + Direction sensor_direction = Direction::CW; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration /** * Function providing BLDCMotor class with the From 10c5b872672cab72df16ddd738bbf09bcce95d28 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 22:14:42 +0200 Subject: [PATCH 026/220] update example for new init logic --- .../find_sensor_offset_and_direction.ino | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 3ad87cca..27163dfb 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -2,8 +2,9 @@ * Simple example intended to help users find the zero offset and natural direction of the sensor. * * These values can further be used to avoid motor and sensor alignment procedure. - * - * motor.initFOC(zero_offset, sensor_direction); + * To use these values add them to the code:"); + * motor.sensor_direction=Direction::CW; // or Direction::CCW + * motor.zero_electric_angle=1.2345; // use the real value! * * This will only work for abosolute value sensors - magnetic sensors. * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors. @@ -44,6 +45,9 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; + // force direction search - because default is CW + motor.sensor_direction = Direction::UNKNOWN; + // initialize motor motor.init(); // align sensor and start FOC @@ -54,9 +58,16 @@ void setup() { Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); - Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW"); + Serial.println(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + + Serial.println("To use these values add them to the code:"); + Serial.print(" motor.sensor_direction="); + Serial.print(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + Serial.println(";"); + Serial.print(" motor.zero_electric_angle="); + Serial.print(motor.zero_electric_angle, 4); + Serial.println(";"); - Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)"); _delay(1000); Serial.println("If motor is not moving the alignment procedure was not successfull!!"); } From 375c36f971bdb853403bbc33d78bbde114b1ee0e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Jun 2023 00:50:48 +0200 Subject: [PATCH 027/220] change sine implementation to deku65i version --- src/BLDCMotor.cpp | 6 +-- src/StepperMotor.cpp | 7 +--- src/common/base_classes/CurrentSense.cpp | 13 ++++-- src/common/foc_utils.cpp | 53 ++++++++++++------------ src/common/foc_utils.h | 7 ++++ 5 files changed, 46 insertions(+), 40 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 45108377..80dacbc3 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -539,12 +539,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { case FOCModulationType::SinePWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index f1e51cce..16df4236 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -351,12 +351,9 @@ void StepperMotor::move(float new_target) { void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation + float _sa, _ca; + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - float _ca = _cos(angle_el); - float _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index becfa499..609162c2 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -38,8 +38,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // if motor angle provided function returns signed value of the current // determine the sign of the current // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 - if(motor_electrical_angle) - sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; + if(motor_electrical_angle) { + float ct; + float st; + _sincos(motor_electrical_angle, &st, &ct); + sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + } // return current magnitude return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); } @@ -78,8 +82,9 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ } // calculate park transform - float ct = _cos(angle_el); - float st = _sin(angle_el); + float ct; + float st; + _sincos(angle_el, &st, &ct); DQCurrent_s return_current; return_current.d = i_alpha * ct + i_beta * st; return_current.q = i_beta * ct - i_alpha * st; diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index c38fd897..93e742a5 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -2,34 +2,29 @@ // function approximating the sine calculation by using fixed size array -// ~40us (float array) -// ~50us (int array) -// precision +-0.005 -// it has to receive an angle in between 0 and 2PI +// uses a 65 element lookup table and interpolation +// thanks to @dekutree for his work on optimizing this __attribute__((weak)) float _sin(float a){ - // int array instead of float array - // 4x200 points per 360 deg - // 2x storage save (int 2Byte float 4 Byte ) - // sin*10000 - static const uint16_t sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; - - if(a < _PI_2){ - //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873f* a)]; // float array optimized - return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized - }else if(a < _PI){ - // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized - return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized - }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized - } else { - // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized + // 16bit integer array for sine lookup. interpolation is used for better precision + // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size + // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) + static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; + unsigned int i = (unsigned int)(a * (64*4*256 /_2PI)); + int t1, t2, frac = i & 0xff; + i = (i >> 8) & 0xff; + if (i < 64) { + t1 = sine_array[i]; t2 = sine_array[i+1]; + } + else if(i < 128) { + t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + } + else if(i < 196) { + t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; } + else { + t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + } + return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } // function approximating cosine calculation by using fixed size array @@ -44,6 +39,12 @@ __attribute__((weak)) float _cos(float a){ } +__attribute__((weak)) void _sincos(float a, float* s, float* c){ + *s = _sin(a); + *c = _cos(a); +} + + // normalizing radian angle to [0,2PI] __attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index c5ff1852..cf3df719 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -71,6 +71,13 @@ float _sin(float a); * @param a angle in between 0 and 2PI */ float _cos(float a); +/** + * Function returning both sine and cosine of the angle in one call. + * Internally it uses the _sin and _cos functions, but you may wish to + * provide your own implementation which is more optimized. + */ +void _sincos(float a, float* s, float* c); + /** * normalizing radian angle to [0,2PI] From 78fd21ee9e340305c2bb6987ca9f9919f161aeee Mon Sep 17 00:00:00 2001 From: Louis Frank Date: Sat, 1 Jul 2023 20:46:30 +0200 Subject: [PATCH 028/220] add currWireError to I2C sensor --- src/sensors/MagneticSensorI2C.cpp | 2 +- src/sensors/MagneticSensorI2C.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index af93b8cc..fb4eee03 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -95,7 +95,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // notify the device that is aboout to be read wire->beginTransmission(chip_address); wire->write(angle_reg_msb); - wire->endTransmission(false); + currWireError = wire->endTransmission(false); // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index fa7ec96b..f8b189fa 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -51,6 +51,9 @@ class MagneticSensorI2C: public Sensor{ /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin , byte scl_pin ); + /** current error code from Wire endTransmission() call **/ + uint8_t currWireError = 0; + private: float cpr; //!< Maximum range of the magnetic sensor uint16_t lsb_used; //!< Number of bits used in LSB register From b2efa9c5b6e5620bc79498013b5439b7bcb7ddeb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 2 Jul 2023 21:33:55 +0200 Subject: [PATCH 029/220] fix addition error, thanks to nano for finding it! --- src/common/foc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 93e742a5..c6a77ae6 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -18,7 +18,7 @@ __attribute__((weak)) float _sin(float a){ else if(i < 128) { t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; } - else if(i < 196) { + else if(i < 192) { t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; } else { From dfa64dc781cd33bc9cb42fc3bd4e8b22bceae4bc Mon Sep 17 00:00:00 2001 From: Louis Frank Date: Mon, 3 Jul 2023 21:47:08 +0200 Subject: [PATCH 030/220] Readme - add dev branch pull request hint --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 424f3cd7..9f13e527 100644 --- a/README.md +++ b/README.md @@ -132,7 +132,9 @@ Please do not hesitate to leave an issue if you have problems/advices/suggestion Pull requests are welcome, but let's first discuss them in [community forum](https://community.simplefoc.com)! -If you'd like to contribute to this porject but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. +If you'd like to contribute to this project but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. + +If you are familiar, we accept pull requests to the dev branch! ## Arduino code example This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder. From 071a98d3e697b0f4fa4ebaf1068bc24049c5f24c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 5 Jul 2023 03:10:57 +0200 Subject: [PATCH 031/220] UNO R4 boards define PinStatus --- 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 5de0783f..352e3b09 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) +#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) #define PinStatus int #endif From 83597c19cf75bdaf45e6457cc07c64de6cb12f66 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 5 Jul 2023 03:12:11 +0200 Subject: [PATCH 032/220] Initial PWM support for Renesas R4M1 MCUs (UNO R4) --- .../hardware_specific/renesas/renesas.cpp | 417 ++++++++++++++++++ .../hardware_specific/renesas/renesas.h | 27 ++ 2 files changed, 444 insertions(+) create mode 100644 src/drivers/hardware_specific/renesas/renesas.cpp create mode 100644 src/drivers/hardware_specific/renesas/renesas.h diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp new file mode 100644 index 00000000..e85a0e68 --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -0,0 +1,417 @@ + +#include "./renesas.h" + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + +#include "communication/SimpleFOCDebug.h" +#include "FspTimer.h" + +/* + We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit) + Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary + + So each timer channel can handle one half-bridge, using either a single or + two complimentary PWM signals. + + For 1-PWM through 4-PWM, we need as many channels as PWM signals, and we can use + either output A or B of the timer (we can set the polarity). + + For 6-PWM we need 3 channels, and use both outputs A and B of each channel, then + we can do hardware dead-time. + Or we can use seperate channels for high and low side, with software dead-time. + + Selected channels can be started together, so we can keep the phases in sync for + low-side current sensing and software 6-PWM. + + The event system should permit low side current sensing without needing interrupts, + but this will be handled by the current sense driver. + */ + + +// track which channels are used +bool channel_used[GPT_HOWMANY] = { false }; + + +struct RenesasTimerConfig { + timer_cfg_t timer_cfg; + gpt_instance_ctrl_t ctrl; + gpt_extended_cfg_t ext_cfg; + gpt_extended_pwm_cfg_t pwm_cfg; + gpt_io_pin_t duty_pin; +}; + +struct ClockDivAndRange { + timer_source_div_t clk_div; + uint32_t range; +}; + +ClockDivAndRange getClockDivAndRange(uint32_t pwm_frequency, uint8_t timer_channel) { + ClockDivAndRange result; + uint32_t max_count = (timer_channel < GTP32_HOWMANY)? 4294967295 : 65535; + uint32_t freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD); + float range = (float) freq_hz / ((float) pwm_frequency * 2.0f); + if(range / 1.0 < max_count) { + result.range = (uint32_t) (range / 1.0); + result.clk_div = TIMER_SOURCE_DIV_1; + } + else if (range / 2.0 < max_count) { + result.range = (uint32_t) (range / 2.0); + result.clk_div = TIMER_SOURCE_DIV_2; + } + else if(range / 4.0 < max_count) { + result.range = (uint32_t) (range / 4.0); + result.clk_div = TIMER_SOURCE_DIV_4; + } + else if(range / 8.0 < max_count) { + result.range = (uint32_t) (range / 8.0 ); + result.clk_div = TIMER_SOURCE_DIV_8; + } + else if(range / 16.0 < max_count) { + result.range = (uint32_t) (range / 16.0 ); + result.clk_div = TIMER_SOURCE_DIV_16; + } + else if (range / 32.0 < max_count) { + result.range = (uint32_t) (range / 32.0 ); + result.clk_div = TIMER_SOURCE_DIV_32; + } + else if(range / 64.0 < max_count) { + result.range = (uint32_t) (range / 64.0 ); + result.clk_div = TIMER_SOURCE_DIV_64; + } + else if(range / 128.0 < max_count) { + result.range = (uint32_t) (range / 128.0 ); + result.clk_div = TIMER_SOURCE_DIV_128; + } + else if(range / 256.0 < max_count) { + result.range = (uint32_t) (range / 256.0 ); + result.clk_div = TIMER_SOURCE_DIV_256; + } + else if(range / 512.0 < max_count) { + result.range = (uint32_t) (range / 512.0 ); + result.clk_div = TIMER_SOURCE_DIV_512; + } + else if(range / 1024.0 < max_count) { + result.range = (uint32_t) (range / 1024.0 ); + result.clk_div = TIMER_SOURCE_DIV_1024; + } + else { + SimpleFOCDebug::println("DRV: PWM frequency too low"); + } + + return result; +}; + + +bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { + uint8_t pin = params->pins[index]; + std::array pinCfgs = getPinCfgs(pin, PIN_CFG_REQ_PWM); + if(pinCfgs[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin"); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs[0])) { + SIMPLEFOC_DEBUG("DRV: AGT timer not supported"); + return false; + } + // get the timer channel + uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]); + // check its not used + if (channel_used[timer_channel]) { + SIMPLEFOC_DEBUG("DRV: channel in use"); + return false; + } + + // configure GPIO pin + // pinMode(pin, OUTPUT); + fsp_err_t err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin config failed"); + return false; + } + + TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; + + + // configure timer channel - frequency / top value + ClockDivAndRange timings = getClockDivAndRange(params->pwm_frequency, timer_channel); + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("---PWM Config---"); + SimpleFOCDebug::println("DRV: pwm pin: ", pin); + SimpleFOCDebug::println("DRV: pwm channel: ", timer_channel); + SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println((pwm_output==CHANNEL_A)?"A":"B"); + SimpleFOCDebug::println("DRV: pwm freq: ", (int)params->pwm_frequency); + SimpleFOCDebug::println("DRV: pwm range: ", (int)timings.range); + SimpleFOCDebug::println("DRV: pwm clkdiv: ", timings.clk_div); + #endif + + RenesasTimerConfig* t = new RenesasTimerConfig(); + // configure timer channel - count mode + t->timer_cfg.channel = timer_channel; + t->timer_cfg.mode = TIMER_MODE_TRIANGLE_WAVE_SYMMETRIC_PWM; + t->timer_cfg.source_div = timings.clk_div; + t->timer_cfg.period_counts = timings.range; + t->timer_cfg.duty_cycle_counts = 0; + t->timer_cfg.p_callback = nullptr; + t->timer_cfg.p_context = nullptr; + t->timer_cfg.p_extend = &(t->ext_cfg); + t->timer_cfg.cycle_end_ipl = BSP_IRQ_DISABLED; + + t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg); + t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED; + t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR; + t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0; + t->pwm_cfg.output_disable = GPT_OUTPUT_DISABLE_NONE; + t->pwm_cfg.adc_trigger = GPT_ADC_TRIGGER_NONE; + t->pwm_cfg.dead_time_count_up = 0; + t->pwm_cfg.dead_time_count_down = 0; + t->pwm_cfg.adc_a_compare_match = 0; + t->pwm_cfg.adc_b_compare_match = 0; + t->pwm_cfg.interrupt_skip_source = GPT_INTERRUPT_SKIP_SOURCE_NONE; + t->pwm_cfg.interrupt_skip_count = GPT_INTERRUPT_SKIP_COUNT_0; + t->pwm_cfg.interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE; + t->pwm_cfg.gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + t->pwm_cfg.gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + + + + // configure timer channel - polarity + t->ext_cfg.gtior_setting.gtior = 0L; + if(pwm_output == CHANNEL_A) { + t->duty_pin = GPT_IO_PIN_GTIOCA; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x08); + t->ext_cfg.gtior_setting.gtior_b.oadflt = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0; + t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0; + t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0; + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCB; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtioca.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x08); + t->ext_cfg.gtior_setting.gtior_b.obdflt = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.obhld = 0x0; + t->ext_cfg.gtior_setting.gtior_b.obdf = 0x0; + t->ext_cfg.gtior_setting.gtior_b.nfben = 0x0; + } + // t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; + // TODO configure timer channel - dead-time if both outputs are used + memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); + + err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); + if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { + SIMPLEFOC_DEBUG("DRV: open failed"); + return false; + } + #if defined(SIMPLEFOC_RESENSAS_DEBUG) + if (err == FSP_ERR_ALREADY_OPEN) { + SimpleFOCDebug::println("DRV: timer already open"); + } + #endif + // err = R_GPT_Enable(&(t->ctrl)); + // if (err != FSP_SUCCESS) { + // SIMPLEFOC_DEBUG("DRV: enable failed"); + // return false; + // } + err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: period set failed"); + return false; + } + err = R_GPT_OutputEnable(&(t->ctrl), t->duty_pin); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin enable failed"); + return false; + } + params->timer_config[index] = t; + params->channels[index] = timer_channel; + channel_used[timer_channel] = true; + + return true; +} + + +bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { + + // start the channels + uint32_t mask = 0; + for (int i = 0; i < num_channels; i++) { + RenesasTimerConfig* t = params->timer_config[i]; + // if (R_GPT_Start(&(t->ctrl)) != FSP_SUCCESS) { + // SIMPLEFOC_DEBUG("DRV: timer start failed"); + // return false; + // } + mask |= (1 << params->channels[i]); +#if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: starting timer: ", params->channels[i]); +#endif + } + params->timer_config[0]->ctrl.p_reg->GTSTR |= mask; + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: timers started"); + #endif + return true; +} + + + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0); + if (success) + success = startTimerChannels(params, 1); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0); + if (success) + success = configureTimerPin(params, 1); + if (success) + success = startTimerChannels(params, 2); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; params->pins[2] = pinC; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0); + if (success) + success = configureTimerPin(params, 1); + if (success) + success = configureTimerPin(params, 2); + if (success) + success = startTimerChannels(params, 3); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pin1A; params->pins[1] = pin1B; params->pins[2] = pin2A; params->pins[3] = pin2B; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0); + if (success) + success = configureTimerPin(params, 1); + if (success) + success = configureTimerPin(params, 2); + if (success) + success = configureTimerPin(params, 3); + if (success) + success = startTimerChannels(params, 4); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +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){ + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + + +void _writeDutyCycle1PWM(float dc_a, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + //SimpleFOCDebug::println("Duty A: ", (int)duty_cycle_counts); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + Serial.println("pwm set error A"); + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + //SimpleFOCDebug::println("Duty B: ", (int)duty_cycle_counts); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + Serial.println("pwm set error B"); + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + //SimpleFOCDebug::println("Duty C: ", (int)duty_cycle_counts); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + Serial.println("pwm set error C"); + } +} + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_1a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_1b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_2a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + duty_cycle_counts = (uint32_t)(dc_2b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/renesas/renesas.h b/src/drivers/hardware_specific/renesas/renesas.h new file mode 100644 index 00000000..a94386e8 --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.h @@ -0,0 +1,27 @@ +#pragma once + + +#include "../../hardware_api.h" + + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + +// uncomment to enable debug output from Renesas driver +// can set this as build-flag in Arduino IDE or PlatformIO +#define SIMPLEFOC_RENESAS_DEBUG + +#define RENESAS_DEFAULT_PWM_FREQUENCY 24000 + +struct RenesasTimerConfig; + +typedef struct RenesasHardwareDriverParams { + uint8_t pins[6]; + uint8_t channels[6]; + RenesasTimerConfig* timer_config[6]; + long pwm_frequency; + float dead_zone; +} RenesasHardwareDriverParams; + + + +#endif \ No newline at end of file From ca129fd78a1c6ec26fd70442b1a64d224102397c Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Sat, 1 Jul 2023 01:52:29 +0200 Subject: [PATCH 033/220] add ESP32 output inversion support --- src/drivers/hardware_specific/esp32/esp32_mcu.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index fecb712c..0afaa1f7 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -41,7 +41,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_config_t pwm_config; pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 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 PWM1A & PWM1B with above settings @@ -50,9 +50,14 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm if (_isset(dead_zone)){ // dead zone is configured float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_type_t pwm_mode; + if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); } _delay(100); From 787cb158b46182d555e2b7f2428c550212a91f0b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 6 Jul 2023 02:44:01 +0200 Subject: [PATCH 034/220] software 6-PWM is working --- .../hardware_specific/renesas/renesas.cpp | 239 +++++++++++++----- .../hardware_specific/renesas/renesas.h | 1 + 2 files changed, 172 insertions(+), 68 deletions(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index e85a0e68..5b68558c 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -97,16 +97,17 @@ ClockDivAndRange getClockDivAndRange(uint32_t pwm_frequency, uint8_t timer_chann else { SimpleFOCDebug::println("DRV: PWM frequency too low"); } - return result; }; -bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { +bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool active_high, bool complementary = false, bool complementary_active_high = true) { uint8_t pin = params->pins[index]; + uint8_t pin_C; std::array pinCfgs = getPinCfgs(pin, PIN_CFG_REQ_PWM); + std::array pinCfgs_C; if(pinCfgs[0] == 0) { - SIMPLEFOC_DEBUG("DRV: no PWM on pin"); + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin); return false; } if (IS_PIN_AGT_PWM(pinCfgs[0])) { @@ -121,24 +122,46 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { return false; } + if (complementary) { + pin_C = params->pins[index+1]; + pinCfgs_C = getPinCfgs(pin_C, PIN_CFG_REQ_PWM); + if(pinCfgs_C[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin_C); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs_C[0]) || GET_CHANNEL(pinCfgs_C[0])!=timer_channel) { + SIMPLEFOC_DEBUG("DRV: comp. channel different"); + return false; + } + } + TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; + if (complementary) { + TimerPWMChannel_t pwm_output_C = IS_PWM_ON_A(pinCfgs_C[0]) ? CHANNEL_A : CHANNEL_B; + if (pwm_output != CHANNEL_A || pwm_output_C != CHANNEL_B) { + SIMPLEFOC_DEBUG("DRV: output A/B mismatch"); + return false; + } + } + // configure GPIO pin - // pinMode(pin, OUTPUT); fsp_err_t err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if ((err == FSP_SUCCESS) && complementary) + err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin_C].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); if (err != FSP_SUCCESS) { SIMPLEFOC_DEBUG("DRV: pin config failed"); return false; } - TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; - // configure timer channel - frequency / top value ClockDivAndRange timings = getClockDivAndRange(params->pwm_frequency, timer_channel); #if defined(SIMPLEFOC_RENESAS_DEBUG) SimpleFOCDebug::println("---PWM Config---"); SimpleFOCDebug::println("DRV: pwm pin: ", pin); + if (complementary) + SimpleFOCDebug::println("DRV: compl. pin: ", pin_C); SimpleFOCDebug::println("DRV: pwm channel: ", timer_channel); - SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println((pwm_output==CHANNEL_A)?"A":"B"); + SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println(complementary?"A+B":((pwm_output==CHANNEL_A)?"A":"B")); SimpleFOCDebug::println("DRV: pwm freq: ", (int)params->pwm_frequency); SimpleFOCDebug::println("DRV: pwm range: ", (int)timings.range); SimpleFOCDebug::println("DRV: pwm clkdiv: ", timings.clk_div); @@ -171,35 +194,45 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { t->pwm_cfg.interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE; t->pwm_cfg.gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; t->pwm_cfg.gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + // configure dead-time if both outputs are used + if (complementary) { + uint32_t dt = params->dead_zone * timings.range; + t->pwm_cfg.dead_time_count_up = dt; + t->pwm_cfg.dead_time_count_down = dt; + } - - - // configure timer channel - polarity + // configure timer channel - outputs and polarity t->ext_cfg.gtior_setting.gtior = 0L; - if(pwm_output == CHANNEL_A) { - t->duty_pin = GPT_IO_PIN_GTIOCA; - t->ext_cfg.gtioca.output_enabled = true; - t->ext_cfg.gtiocb.output_enabled = false; - t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x08); - t->ext_cfg.gtior_setting.gtior_b.oadflt = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x01; - t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0; - t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0; - t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0; - } + if (!complementary) { + if(pwm_output == CHANNEL_A) { + t->duty_pin = GPT_IO_PIN_GTIOCA; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01; + // t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0; + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCB; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtioca.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; + } + } else { - t->duty_pin = GPT_IO_PIN_GTIOCB; - t->ext_cfg.gtiocb.output_enabled = true; - t->ext_cfg.gtioca.output_enabled = false; - t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x08); - t->ext_cfg.gtior_setting.gtior_b.obdflt = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0x00 : 0x01; - t->ext_cfg.gtior_setting.gtior_b.obhld = 0x0; - t->ext_cfg.gtior_setting.gtior_b.obdf = 0x0; - t->ext_cfg.gtior_setting.gtior_b.nfben = 0x0; - } - // t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; - // TODO configure timer channel - dead-time if both outputs are used - memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); + t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (complementary_active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = complementary_active_high ? 0x00 : 0x01; + } + memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { SIMPLEFOC_DEBUG("DRV: open failed"); @@ -210,11 +243,6 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { SimpleFOCDebug::println("DRV: timer already open"); } #endif - // err = R_GPT_Enable(&(t->ctrl)); - // if (err != FSP_SUCCESS) { - // SIMPLEFOC_DEBUG("DRV: enable failed"); - // return false; - // } err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts); if (err != FSP_SUCCESS) { SIMPLEFOC_DEBUG("DRV: period set failed"); @@ -225,17 +253,21 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index) { SIMPLEFOC_DEBUG("DRV: pin enable failed"); return false; } + + channel_used[timer_channel] = true; params->timer_config[index] = t; params->channels[index] = timer_channel; - channel_used[timer_channel] = true; + if (complementary) { + params->timer_config[index+1] = t; + params->channels[index+1] = timer_channel; + } return true; } +// start the timer channels for the motor, synchronously bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { - - // start the channels uint32_t mask = 0; for (int i = 0; i < num_channels; i++) { RenesasTimerConfig* t = params->timer_config[i]; @@ -256,6 +288,18 @@ bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { } +// check if the given pins are on the same timer channel +bool isHardware6Pwm(const int pin1, const int pin2) { + std::array pinCfgs1 = getPinCfgs(pin1, PIN_CFG_REQ_PWM); + std::array pinCfgs2 = getPinCfgs(pin2, PIN_CFG_REQ_PWM); + if(pinCfgs1[0] == 0 || pinCfgs2[0] == 0) + return false; + if (IS_PIN_AGT_PWM(pinCfgs1[0]) || IS_PIN_AGT_PWM(pinCfgs2[0])) + return false; + uint8_t timer_channel1 = GET_CHANNEL(pinCfgs1[0]); + uint8_t timer_channel2 = GET_CHANNEL(pinCfgs2[0]); + return timer_channel1==timer_channel2; +} @@ -264,7 +308,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { params->pins[0] = pinA; params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; bool success = true; - success = configureTimerPin(params, 0); + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); if (success) success = startTimerChannels(params, 1); if (!success) @@ -278,11 +322,10 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { params->pins[0] = pinA; params->pins[1] = pinB; params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; bool success = true; - success = configureTimerPin(params, 0); - if (success) - success = configureTimerPin(params, 1); - if (success) - success = startTimerChannels(params, 2); + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (!success) + success &= startTimerChannels(params, 2); if (!success) return SIMPLEFOC_DRIVER_INIT_FAILED; return params; @@ -294,11 +337,9 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in params->pins[0] = pinA; params->pins[1] = pinB; params->pins[2] = pinC; params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; bool success = true; - success = configureTimerPin(params, 0); - if (success) - success = configureTimerPin(params, 1); - if (success) - success = configureTimerPin(params, 2); + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); if (success) success = startTimerChannels(params, 3); if (!success) @@ -312,13 +353,10 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const params->pins[0] = pin1A; params->pins[1] = pin1B; params->pins[2] = pin2A; params->pins[3] = pin2B; params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; bool success = true; - success = configureTimerPin(params, 0); - if (success) - success = configureTimerPin(params, 1); - if (success) - success = configureTimerPin(params, 2); - if (success) - success = configureTimerPin(params, 3); + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, SIMPLEFOC_PWM_ACTIVE_HIGH); if (success) success = startTimerChannels(params, 4); if (!success) @@ -328,10 +366,38 @@ 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){ - GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .pwm_frequency = pwm_frequency - }; + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA_h; params->pins[1] = pinA_l; params->pins[2] = pinB_h; params->pins[3] = pinB_l; params->pins[4] = pinC_h; params->pins[5] = pinC_l; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + params->dead_zone = (dead_zone==NOT_SET)?RENESAS_DEFAULT_DEAD_ZONE:dead_zone; + + bool success = true; + if (isHardware6Pwm(pinA_h, pinA_l)) { + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + } + else { + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); // reverse polarity on low side gives desired active high/low behaviour + } + if (isHardware6Pwm(pinB_h, pinB_l)) { + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + } + else { + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + } + if (isHardware6Pwm(pinC_h, pinC_l)) { + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + } + else { + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 5, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + } + + if (success) + success = startTimerChannels(params, 6); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; return params; } @@ -364,24 +430,18 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); - //SimpleFOCDebug::println("Duty A: ", (int)duty_cycle_counts); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { // error - Serial.println("pwm set error A"); } t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); - //SimpleFOCDebug::println("Duty B: ", (int)duty_cycle_counts); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { // error - Serial.println("pwm set error B"); } t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); - //SimpleFOCDebug::println("Duty C: ", (int)duty_cycle_counts); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { // error - Serial.println("pwm set error C"); } } @@ -410,7 +470,50 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo } + // TODO phase-state void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t dt = (uint32_t)(((RenesasHardwareDriverParams*)params)->dead_zone * (float)(t->timer_cfg.period_counts)); + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + bool hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[0] == ((RenesasHardwareDriverParams*)params)->channels[1]; + uint32_t dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[2] == ((RenesasHardwareDriverParams*)params)->channels[3]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + t = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[4]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[4] == ((RenesasHardwareDriverParams*)params)->channels[5]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + t = ((RenesasHardwareDriverParams*)params)->timer_config[5]; + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + } } diff --git a/src/drivers/hardware_specific/renesas/renesas.h b/src/drivers/hardware_specific/renesas/renesas.h index a94386e8..91bacdce 100644 --- a/src/drivers/hardware_specific/renesas/renesas.h +++ b/src/drivers/hardware_specific/renesas/renesas.h @@ -11,6 +11,7 @@ #define SIMPLEFOC_RENESAS_DEBUG #define RENESAS_DEFAULT_PWM_FREQUENCY 24000 +#define RENESAS_DEFAULT_DEAD_ZONE 0.05f struct RenesasTimerConfig; From 57be301ccddad3ea958c95c0be298ea038ea2952 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 6 Jul 2023 18:18:55 +0200 Subject: [PATCH 035/220] hardware 6-PWM is working --- src/drivers/hardware_specific/renesas/renesas.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index 5b68558c..e4241784 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -137,7 +137,7 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; if (complementary) { TimerPWMChannel_t pwm_output_C = IS_PWM_ON_A(pinCfgs_C[0]) ? CHANNEL_A : CHANNEL_B; - if (pwm_output != CHANNEL_A || pwm_output_C != CHANNEL_B) { + if (pwm_output_C != CHANNEL_A || pwm_output != CHANNEL_B) { SIMPLEFOC_DEBUG("DRV: output A/B mismatch"); return false; } @@ -373,21 +373,21 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons bool success = true; if (isHardware6Pwm(pinA_h, pinA_l)) { - success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 0, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); } else { success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); success &= configureTimerPin(params, 1, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); // reverse polarity on low side gives desired active high/low behaviour } if (isHardware6Pwm(pinB_h, pinB_l)) { - success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 2, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); } else { success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); success &= configureTimerPin(params, 3, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); } if (isHardware6Pwm(pinC_h, pinC_l)) { - success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 4, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); } else { success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); From 8010c3dd6340e8349775e2d8d96de641c3ad2a75 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 6 Jul 2023 21:35:10 +0200 Subject: [PATCH 036/220] clean up polarity --- .../hardware_specific/renesas/renesas.cpp | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index e4241784..c60c4eac 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -226,10 +226,10 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; t->ext_cfg.gtioca.output_enabled = true; t->ext_cfg.gtiocb.output_enabled = true; - t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10); - t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01; - t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (complementary_active_high ? 0x00 : 0x10); - t->ext_cfg.gtior_setting.gtior_b.obdflt = complementary_active_high ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (!complementary_active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = !complementary_active_high ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; } memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); @@ -372,26 +372,25 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons params->dead_zone = (dead_zone==NOT_SET)?RENESAS_DEFAULT_DEAD_ZONE:dead_zone; bool success = true; - if (isHardware6Pwm(pinA_h, pinA_l)) { - success &= configureTimerPin(params, 0, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); - } + if (isHardware6Pwm(pinA_h, pinA_l)) + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); else { success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); - success &= configureTimerPin(params, 1, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); // reverse polarity on low side gives desired active high/low behaviour - } - if (isHardware6Pwm(pinB_h, pinB_l)) { - success &= configureTimerPin(params, 2, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 1, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); // reverse polarity on low side gives desired active high/low behaviour } + + if (isHardware6Pwm(pinB_h, pinB_l)) + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); else { success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); - success &= configureTimerPin(params, 3, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); - } - if (isHardware6Pwm(pinC_h, pinC_l)) { - success &= configureTimerPin(params, 4, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 3, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } + + if (isHardware6Pwm(pinC_h, pinC_l)) + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); else { success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); - success &= configureTimerPin(params, 5, !(SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH)); + success &= configureTimerPin(params, 5, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } if (success) From 4351a6ccabf26e17c6297afc7bd713d94dcf0c4f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 7 Jul 2023 11:19:10 +0200 Subject: [PATCH 037/220] Renesas PWM phase state setting support --- .../hardware_specific/renesas/renesas.cpp | 94 ++++++++++++++++--- 1 file changed, 83 insertions(+), 11 deletions(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index c60c4eac..91dfa532 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -8,23 +8,40 @@ /* We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit) - Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary + Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary. - So each timer channel can handle one half-bridge, using either a single or - two complimentary PWM signals. + So each timer channel can handle one half-bridge, using either a single (3-PWM) or + two complimentary PWM signals (6-PWM). For 1-PWM through 4-PWM, we need as many channels as PWM signals, and we can use - either output A or B of the timer (we can set the polarity). + either output A or B of the timer (we can set the polarity) - but not both. For 6-PWM we need 3 channels, and use both outputs A and B of each channel, then we can do hardware dead-time. Or we can use seperate channels for high and low side, with software dead-time. + Each phase can be either hardware (1 channel) or software (2 channels) dead-time + individually, they don't all have to be one or the other. Selected channels can be started together, so we can keep the phases in sync for low-side current sensing and software 6-PWM. The event system should permit low side current sensing without needing interrupts, but this will be handled by the current sense driver. + + Supported: + - arbitrary PWM frequencies between 1Hz (minimum we can set with our integer based API) + and around 48kHz (more would be possible but the range will be low) + - PWM range at 24kHz (default) is 1000 + - PWM range at 48kHz is 500 + - polarity setting is supported, in all modes + - phase state setting is supported, in 3-PWM, 6-PWM hardware dead-time and 6-PWM software dead-time + + TODOs: + - change setDutyCycle to use register access for speed + - add event system support for low-side current sensing + - perhaps add support to reserve timers used also in + Arduino Pwm.h code, for compatibility with analogWrite() + - check if there is a better way for phase-state setting */ @@ -469,47 +486,102 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo } - // TODO phase-state + +void _setSinglePhaseState(RenesasTimerConfig* hi, RenesasTimerConfig* lo, PhaseState state) { + gpt_gtior_setting_t gtior; + gtior.gtior = hi->ctrl.p_reg->GTIOR; + bool on = (state==PHASE_ON) || (state==PHASE_HI); + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA_AND_GTIOCB) { + bool ch = false; + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + ch = true; + } // B is high side + on = (state==PHASE_ON) || (state==PHASE_LO); + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + ch = true; + } + if (ch) + hi->ctrl.p_reg->GTIOR = gtior.gtior; + return; + } + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (hi->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + + gtior.gtior = lo->ctrl.p_reg->GTIOR; + on = (state==PHASE_ON) || (state==PHASE_LO); + if (lo->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (lo->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + +} + + void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + RenesasTimerConfig* t1 = ((RenesasHardwareDriverParams*)params)->timer_config[1]; uint32_t dt = (uint32_t)(((RenesasHardwareDriverParams*)params)->dead_zone * (float)(t->timer_cfg.period_counts)); uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); bool hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[0] == ((RenesasHardwareDriverParams*)params)->channels[1]; uint32_t dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[0]); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { // error } if (!hw_deadtime) { - t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; - if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { // error } } t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[3]; duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[2] == ((RenesasHardwareDriverParams*)params)->channels[3]; dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[1]); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { // error } if (!hw_deadtime) { - t = ((RenesasHardwareDriverParams*)params)->timer_config[3]; - if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { // error } } t = ((RenesasHardwareDriverParams*)params)->timer_config[4]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[5]; duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[4] == ((RenesasHardwareDriverParams*)params)->channels[5]; dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[2]); if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { // error } if (!hw_deadtime) { - t = ((RenesasHardwareDriverParams*)params)->timer_config[5]; - if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts + dt_act, t->duty_pin) != FSP_SUCCESS) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { // error } } From bb001e9cfeda9b657b220cd26996de5506aa8b41 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 7 Jul 2023 17:12:59 +0200 Subject: [PATCH 038/220] prepare README for next release --- README.md | 40 +++++++++++----------------------------- 1 file changed, 11 insertions(+), 29 deletions(-) diff --git a/README.md b/README.md index 424f3cd7..47d7540f 100644 --- a/README.md +++ b/README.md @@ -24,34 +24,16 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.0 -> - Arduino Mega 6pwm more timers supported -> - Arduino boards - frequency change support either 32kHz or 4kHz -> - Arduino Uno - synched timers in 3pwm and 6pwm mode [#71](https://github.com/simplefoc/Arduino-FOC/issues/71) -> - Teensy 3.x initial support for 6pwm -> - Teensy 4.x initial support for 6pwm -> - Example for v3.1 SimpleFOCShield -> - RP2040 compatibility for earlehillpower core [#234](https://github.com/simplefoc/Arduino-FOC/pull/234) [#236](https://github.com/simplefoc/Arduino-FOC/pull/236) -> - More flexible monitoring API -> - start, end and separator characters -> - decimal places (settable through commander) -> - Added machine readable verbose mode in `Commander` [#233](https://github.com/simplefoc/Arduino-FOC/pull/233) -> - *Simple**FOC**WebController* - Web based user interface for SimpleFOC by [@geekuillaume](https://github.com/geekuillaume) - [webcontroller.simplefoc.com](webcontroller.simplefoc.com) -> - bugfix - `MagneticSensorPWM` multiple occasions - [#258](https://github.com/simplefoc/Arduino-FOC/pull/258) -> - bugfix - current sense align - added offset exchange when exchanging pins -> - bugfix - trapezoid 150 fixed -> - bugfix - 4pwm on ESP8266 [#224](https://github.com/simplefoc/Arduino-FOC/pull/224) -> - Additional `InlineCurrentSense` and `LowsideCurrentSense` constructor using milliVolts per Amp [#253](https://github.com/simplefoc/Arduino-FOC/pull/253) -> - STM32L4xx current sense support by [@Triple6](https://github.com/Triple6) (discord) [#257](https://github.com/simplefoc/Arduino-FOC/pull/257) -> - phase disable in 6pwm mode -> - stm32 - software and hardware 6pwm -> - atmega328 -> - atmega2560 -> - Lag compensation using motor inductance [#246](https://github.com/simplefoc/Arduino-FOC/issues/246) -> - current control through voltage torque mode enhancement -> - extended `BLDCMotor` and `StepperMotor` constructors to receive the inductance paramerer -> - can also be set using `motor.phase_inductance` or through `Commander` -## Arduino *SimpleFOClibrary* v2.3 +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.1 +> - Support for Arduino UNO R4 Minima and UNO R4 WiFi (Renesas R7FA4M1 MCUs) +> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells)) +> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) +> - Improved default trig functions (sine, cosine) - faster, smaller +> - Overridable trig functions - plug in your own optimized versions +> - bugfix: microseconds overflow in velocity mode +> - bugfix: KV initialization + +## Arduino *SimpleFOClibrary* v2.3.1

@@ -78,7 +60,7 @@ This video demonstrates the *Simple**FOC**library* basic usage, electronic conne - **Cross-platform**: - Seamless code transfer from one microcontroller family to another - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers): - - Arduino: UNO, MEGA, DUE, Leonardo .... + - Arduino: UNO R4, UNO, MEGA, DUE, Leonardo, Nano, Nano33 .... - STM32 - ESP32 - Teensy From 0595b9f3f7890636c17fd13a85c299c0ef8f381b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 Jul 2023 12:39:33 +0200 Subject: [PATCH 039/220] fix bug in new sine for 8 bit machines --- src/common/foc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index c6a77ae6..4cb09863 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { From b3f2e969d0f2de514c591ab7362c9a22bbab439b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 Jul 2023 11:15:09 +0200 Subject: [PATCH 040/220] initialize sensor direction open vs. closed loop --- src/BLDCMotor.cpp | 8 ++++++++ src/StepperMotor.cpp | 8 ++++++++ src/common/base_classes/FOCMotor.h | 2 +- 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 80dacbc3..f8cb8cfa 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -92,6 +92,14 @@ void BLDCMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 16df4236..196f6379 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -56,6 +56,14 @@ void StepperMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 56e7a516..318be99a 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -205,7 +205,7 @@ class FOCMotor // sensor related variabels float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available - Direction sensor_direction = Direction::CW; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration + Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration /** * Function providing BLDCMotor class with the From 012608cf6af8407b287c4ae5ef544a1748d765b2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 Jul 2023 11:19:17 +0200 Subject: [PATCH 041/220] fix typo --- src/BLDCMotor.cpp | 1 - src/StepperMotor.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index f8cb8cfa..c3d34f30 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -97,7 +97,6 @@ void BLDCMotor::init() { ||controller==MotionControlType::velocity_openloop) && (sensor_direction == Direction::UNKNOWN)) { sensor_direction = Direction::CW; - } } _delay(500); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 196f6379..3b103033 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -61,7 +61,6 @@ void StepperMotor::init() { ||controller==MotionControlType::velocity_openloop) && (sensor_direction == Direction::UNKNOWN)) { sensor_direction = Direction::CW; - } } _delay(500); From dbc28d42f12c1c080d93c697b1545b4e3c7bfc4c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 20 Aug 2023 10:05:12 +0200 Subject: [PATCH 042/220] nrf52 compile error fixed --- 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 352e3b09..114e6106 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) +#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) #define PinStatus int #endif From a163d144fd2cee97423da27e9a008a2f47e7fc02 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 21 Aug 2023 23:03:24 +0200 Subject: [PATCH 043/220] esp32 nano insists on 3 parameters for pulseIn --- src/sensors/MagneticSensorPWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 3b0cd9aa..cf211644 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -85,7 +85,7 @@ float MagneticSensorPWM::getSensorAngle(){ int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse - pulse_length_us = pulseIn(pinPWM, HIGH); + pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable? } return pulse_length_us; } From 7d3dbd8a8388353c6e598c6642d7476a3c5a86b0 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 21 Aug 2023 23:55:22 +0200 Subject: [PATCH 044/220] make compatible with Nano ESP32 platform --- src/drivers/hardware_specific/generic_mcu.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index d92004e9..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,7 +1,8 @@ + #include "../hardware_api.h" // if the mcu doen't have defiend analogWrite -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(analogWrite) __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif From 6efd1264df9bc32d2ce51e3e6527c97996146c2b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 21 Aug 2023 23:57:27 +0200 Subject: [PATCH 045/220] use new version of header file --- .../hardware_specific/esp32/esp32s_adc_driver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 2e956770..a2f58ac3 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -6,7 +6,8 @@ #include "freertos/task.h" #include "rom/ets_sys.h" #include "esp_attr.h" -#include "esp_intr.h" +//#include "esp_intr.h" // deprecated +#include "esp_intr_alloc.h" #include "soc/rtc_io_reg.h" #include "soc/rtc_cntl_reg.h" #include "soc/sens_reg.h" From 3dcb7e8f5a8cf7da83930dc3049cce7ce352aa25 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 21 Aug 2023 23:57:54 +0200 Subject: [PATCH 046/220] prevent compile errors when LEDC is forced --- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index dd336416..3df9dff6 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "esp32_adc_driver.h" From 6372a50ac2548a647e3d08cd6876f5b62d26f723 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 29 Aug 2023 23:53:34 +0200 Subject: [PATCH 047/220] add MEGAAVR to archs which define PinStatus --- 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 114e6106..f9691fd3 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) +#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR) #define PinStatus int #endif From a5ecace16f8c180c425818b2e9946fda0637afc6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 29 Aug 2023 23:54:45 +0200 Subject: [PATCH 048/220] add compiler message which driver gets compiled --- src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp | 6 ++++++ src/drivers/hardware_specific/atmega/atmega328_mcu.cpp | 4 ++++ src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp | 4 ++++ src/drivers/hardware_specific/due_mcu.cpp | 6 ++++++ src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h | 7 +++++++ src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp | 4 ++++ src/drivers/hardware_specific/esp8266_mcu.cpp | 6 ++++++ src/drivers/hardware_specific/nrf52_mcu.cpp | 4 ++++ src/drivers/hardware_specific/portenta_h7_mcu.cpp | 6 ++++++ src/drivers/hardware_specific/renesas/renesas.cpp | 7 +++++++ src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 6 ++++++ src/drivers/hardware_specific/samd/samd21_mcu.cpp | 4 ++++ src/drivers/hardware_specific/samd/samd51_mcu.cpp | 5 +++++ src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 4 ++++ src/drivers/hardware_specific/teensy/teensy3_mcu.cpp | 4 ++++ src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 ++++++ 16 files changed, 83 insertions(+) diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 5ad3653c..8a7fbbec 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__AVR_ATmega2560__) || defined(AVR_ATmega1280) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega2560 or Arduino/ATmega1280") +#pragma message("") + + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 385f6e64..53fb1086 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -2,6 +2,10 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega328 ATmega168 ATmega328PB") +#pragma message("") + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b9836e16..b4ad3101 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -3,6 +3,10 @@ #if defined(__AVR_ATmega32U4__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega32U4") +#pragma message("") + // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ // High PWM frequency diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 80ef82d9..4397114b 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__arm__) && defined(__SAM3X8E__) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Due") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4f8ad131..188da8c3 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,6 +5,13 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") + + #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 76027459..a454c052 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -2,6 +2,10 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 LEDC driver") +#pragma message("") + #include "driver/ledc.h" #define _PWM_FREQUENCY 25000 // 25khz diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 15b9c82d..23d94110 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -2,6 +2,12 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP8266") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index f53eada0..a20b828a 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -3,6 +3,10 @@ #if defined(NRF52_SERIES) +#pragma message("") +#pragma message("SimpleFOC: compiling for NRF52") +#pragma message("") + #define PWM_CLK (16000000) #define PWM_FREQ (40000) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b426f0ed..ad16646a 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -3,6 +3,12 @@ #if defined(TARGET_PORTENTA_H7) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Portenta_H7") +#pragma message("") + + #include "pwmout_api.h" #include "pinDefinitions.h" #include "platform/mbed_critical.h" diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index 91dfa532..4a6a3e92 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -3,6 +3,13 @@ #if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Renesas (UNO R4)") +#pragma message("") + + + #include "communication/SimpleFOCDebug.h" #include "FspTimer.h" diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index ebe1a7dd..eee5797b 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -4,6 +4,12 @@ */ #if defined(TARGET_RP2040) + +#pragma message("") +#pragma message("SimpleFOC: compiling for RP2040") +#pragma message("") + + #define SIMPLEFOC_DEBUG_RP2040 #include "../../hardware_api.h" diff --git a/src/drivers/hardware_specific/samd/samd21_mcu.cpp b/src/drivers/hardware_specific/samd/samd21_mcu.cpp index d9bb5b9a..d59a3098 100644 --- a/src/drivers/hardware_specific/samd/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd21_mcu.cpp @@ -7,6 +7,10 @@ #ifdef _SAMD21_ +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD21") +#pragma message("") + #ifndef TCC3_CH0 #define TCC3_CH0 NOT_ON_TIMER diff --git a/src/drivers/hardware_specific/samd/samd51_mcu.cpp b/src/drivers/hardware_specific/samd/samd51_mcu.cpp index 4bdf036b..71bf0b81 100644 --- a/src/drivers/hardware_specific/samd/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd51_mcu.cpp @@ -6,6 +6,11 @@ #if defined(_SAMD51_)||defined(_SAME51_) +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD51/SAME51") +#pragma message("") + + // expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency. // for custom boards or overclockers you can override it using this define. diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index acf6d22b..65dad9f4 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,6 +4,10 @@ #if defined(_STM32_DEF_) +#pragma message("") +#pragma message("SimpleFOC: compiling for STM32") +#pragma message("") + //#define SIMPLEFOC_STM32_DEBUG diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp index de48dad0..3a7b5de5 100644 --- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -10,6 +10,10 @@ #if defined(__arm__) && defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)) +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 3.x") +#pragma message("") + // pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c #if defined(__MK20DX128__) #define FTM0_CH0_PIN 22 diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 9ed33443..322d5a34 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -7,6 +7,12 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + // half_cycle of the PWM variable int half_cycle = 0; From 574ce6339460c37428051814eed50f31cfc5dd29 Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Sat, 2 Sep 2023 17:41:00 +0200 Subject: [PATCH 049/220] Add ESP32 Phasestate with sw-deadtime --- .../esp32/esp32_driver_mcpwm.h | 1 + .../hardware_specific/esp32/esp32_mcu.cpp | 63 +++++++++++++------ 2 files changed, 46 insertions(+), 18 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4f8ad131..806d0577 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -81,6 +81,7 @@ typedef struct ESP32MCPWMDriverParams { mcpwm_unit_t mcpwm_unit; mcpwm_operator_t mcpwm_operator1; mcpwm_operator_t mcpwm_operator2; + float deadtime; } ESP32MCPWMDriverParams; diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 0afaa1f7..2cbf5e24 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -49,15 +49,28 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm if (_isset(dead_zone)){ // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_type_t pwm_mode; - if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); + + // When using hardware deadtime, setting the phase_state parameter is not supported. + #if SIMPLEFOC_ESP32_HW_DEADTIME == true + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_type_t pwm_mode; + if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); + #else // Software deadtime + for (int i = 0; i < 3; i++){ + if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside + else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver + + if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside + else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver + } + #endif + } _delay(100); @@ -374,7 +387,10 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator1, + .mcpwm_operator2 = m_slot.mcpwm_operator2, + .deadtime = _isset(dead_zone) ? dead_zone : 0 }; return params; } @@ -386,15 +402,26 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // se the PWM on the slot timers + // set the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - _UNUSED(phase_state); + #if SIMPLEFOC_ESP32_HW_DEADTIME == true + // Hardware deadtime does deadtime insertion internally + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); + _UNUSED(phase_state); + #else + float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + #endif } #endif From 2252605bd0318bfecff05ee60fcb8067e15a1fa0 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 2 Sep 2023 23:01:43 +0200 Subject: [PATCH 050/220] fix errors and warnings on SIMPLEFOC_DISABLE_DEBUG --- src/BLDCMotor.cpp | 13 +++++++------ src/StepperMotor.cpp | 13 ++++++++----- src/common/base_classes/FOCMotor.cpp | 2 ++ 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c3d34f30..b72728bc 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -167,7 +167,7 @@ int BLDCMotor::initFOC() { exit_flag *= alignCurrentSense(); } } - else SIMPLEFOC_DEBUG("MOT: No current sense."); + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } } if(exit_flag){ @@ -252,10 +252,11 @@ int BLDCMotor::alignSensor() { // check pole pair number if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip dir calib."); } // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -276,7 +277,7 @@ int BLDCMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -305,8 +306,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + if(sensor->needsSearch()) { SIMPLEFOC_DEBUG("MOT: Error: Not found!"); } + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 3b103033..491a20ee 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -114,7 +114,7 @@ int StepperMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - }else SIMPLEFOC_DEBUG("MOT: No sensor."); + } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -177,10 +177,13 @@ int StepperMotor::alignSensor() { float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -200,7 +203,7 @@ int StepperMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -229,7 +232,7 @@ int StepperMotor::absoluteZeroSearch() { // check if the zero found if(monitor_port){ if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 7f48037f..d1427bcf 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -82,8 +82,10 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print + #ifndef SIMPLEFOC_DISABLE_DEBUG SimpleFOCDebug::enable(&print); SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); + #endif } // utility function intended to be used with serial plotter to monitor motor variables From 5bbba0c8a9f6e5432f0e73776035cff31312d6aa Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 3 Sep 2023 12:20:12 +0200 Subject: [PATCH 051/220] added comments for reverse direction #254 --- .../open_loop_position_example/open_loop_position_example.ino | 1 + .../open_loop_velocity_example/open_loop_velocity_example.ino | 1 + 2 files changed, 2 insertions(+) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 83b1fb32..5a915dd8 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -62,6 +62,7 @@ void setup() { void loop() { // open loop angle movements // using motor.voltage_limit and motor.velocity_limit + // angles can be positive or negative, negative angles correspond to opposite motor direction motor.move(target_position); // user communication diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 84f87ab9..43fc6f73 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -60,6 +60,7 @@ void loop() { // open loop velocity movement // using motor.voltage_limit and motor.velocity_limit + // to turn the motor "backwards", just set a negative target_velocity motor.move(target_velocity); // user communication From b54827bd322b60bb6727a5de972db5d06026255b Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Thu, 7 Sep 2023 19:07:43 +0200 Subject: [PATCH 052/220] ESP32: Use hw-deadtime by default --- src/drivers/hardware_specific/esp32/esp32_mcu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 2cbf5e24..0dc23c10 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -2,6 +2,10 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif + // define bldc motor slots array bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A From 7677c855de85f2e756fa23fb9003bc39968881f5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 10 Sep 2023 20:21:54 +0200 Subject: [PATCH 053/220] swap pow() function for _powtwo() --- src/common/foc_utils.h | 1 + src/sensors/MagneticSensorI2C.cpp | 4 ++-- src/sensors/MagneticSensorSPI.cpp | 6 +++--- src/sensors/MagneticSensorSPI.h | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index cf3df719..0efe3b59 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,6 +12,7 @@ #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) #define _UNUSED(v) (void) (v) +#define _powtwo(x) (1 << (x)) // utility defines #define _2_SQRT3 1.15470053838f diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index fb4eee03..ac2b273b 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -28,7 +28,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) - cpr = pow(2, _bit_resolution); + cpr = _powtwo(_bit_resolution); // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits @@ -48,7 +48,7 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ // angle read register of the magnetic sensor angle_register_msb = config.angle_register; // register maximum value (counts per revolution) - cpr = pow(2, config.bit_resolution); + cpr = _powtwo(config.bit_resolution); int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 7341c89a..52febc38 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -31,13 +31,13 @@ MagneticSensorSPIConfig_s MA730_SPI = { // cs - SPI chip select pin // _bit_resolution sensor resolution bit number // _angle_register - (optional) angle read register - default 0x3FFF -MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ +MagneticSensorSPI::MagneticSensorSPI(int cs, int _bit_resolution, int _angle_register){ chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) - cpr = pow(2,_bit_resolution); + cpr = _powtwo(_bit_resolution); spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; @@ -52,7 +52,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ // angle read register of the magnetic sensor angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) - cpr = pow(2, config.bit_resolution); + cpr = _powtwo(config.bit_resolution); spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index a77e7698..03ebde44 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -30,7 +30,7 @@ class MagneticSensorSPI: public Sensor{ * @param bit_resolution sensor resolution bit number * @param angle_register (optional) angle read register - default 0x3FFF */ - MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0); + MagneticSensorSPI(int cs, int bit_resolution, int angle_register = 0); /** * MagneticSensorSPI class constructor * @param config SPI config From 5ef4d9d5a92e03da0dd5af7f624243ab30f1b688 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 12 Sep 2023 18:26:19 +0200 Subject: [PATCH 054/220] add some checks to current-sense init --- src/current_sense/GenericCurrentSense.cpp | 2 ++ src/current_sense/InlineCurrentSense.cpp | 8 ++++++++ src/current_sense/LowsideCurrentSense.cpp | 11 ++++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index d0592ef2..635535fa 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -62,6 +62,8 @@ int GenericCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; + if (!initialized) return 0; + // // set phase A active and phases B and C down // driver->setPwm(voltage, 0, 0); // _delay(200); diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 6a41484a..492b3ac9 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -1,4 +1,5 @@ #include "InlineCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // InlineCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -93,6 +94,13 @@ int InlineCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: No driver linked!"); + return 0; + } + + if (!initialized) return 0; + if(_isset(pinA)){ // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 21df16e1..a706beeb 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -1,4 +1,5 @@ #include "LowsideCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // LowsideCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -35,6 +36,12 @@ LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int // Lowside sensor init function int LowsideCurrentSense::init(){ + + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: Driver not linked!"); + return 0; + } + // configure ADC variables params = _configureADCLowSide(driver->params,pinA,pinB,pinC); // if init failed return fail @@ -89,10 +96,12 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted int LowsideCurrentSense::driverAlign(float voltage){ - + int exit_flag = 1; if(skip_align) return exit_flag; + if (!initialized) return 0; + if(_isset(pinA)){ // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); From 132e7b0d90dc429ff9db208bffcaea82d4a8d40d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 12 Sep 2023 18:26:30 +0200 Subject: [PATCH 055/220] mark param unused --- src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 613c6323..d84c2fd5 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -25,6 +25,7 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-( // like this we either have to block interrupts, or of course have the chance of reading across // new ADC conversions, which probably won't improve the accuracy. + _UNUSED(cs_params); if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) { return engine.lastResults.raw[pinA-26]*engine.adc_conv; @@ -36,6 +37,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) { + _UNUSED(driver_params); + if( _isset(pinA) ) engine.addPin(pinA); if( _isset(pinB) ) From e60cdf5b17942c15e20ebf1c7903fd76e1ad7118 Mon Sep 17 00:00:00 2001 From: candas Date: Wed, 13 Sep 2023 22:23:41 +0200 Subject: [PATCH 056/220] SVPWM with midpoint clamp --- src/BLDCMotor.cpp | 106 +++++++--------------------------------------- 1 file changed, 16 insertions(+), 90 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index b72728bc..de76ce0f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -545,6 +545,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SinePWM : + case FOCModulationType::SpaceVectorPWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation _sincos(angle_el, &_sa, &_ca); @@ -553,107 +554,32 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; - // center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - center = driver->voltage_limit/2; // Clarke transform - Ua = Ualpha + center; - Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; + Ua = Ualpha; + Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta; + Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta; + + center = driver->voltage_limit/2; + if (foc_modulation == FOCModulationType::SpaceVectorPWM){ + // Midpoint Clamp + float Umin = min(Ua, min(Ub, Uc)); + float Umax = max(Ua, max(Ub, Uc)); + center -= (Umax+Umin) / 2; + } if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); Ua -= Umin; Ub -= Umin; Uc -= Umin; + }else{ + Ua += center; + Ub += center; + Uc += center; } break; - case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm - // https://www.youtube.com/watch?v=QMSWUMEAejg - - // the algorithm goes - // 1) Ualpha, Ubeta - // 2) Uout = sqrt(Ualpha^2 + Ubeta^2) - // 3) angle_el = atan2(Ubeta, Ualpha) - // - // equivalent to 2) because the magnitude does not change is: - // Uout = sqrt(Ud^2 + Uq^2) - // equivalent to 3) is - // angle_el = angle_el + atan2(Uq,Ud) - - float Uout; - // a bit of optitmisation - if(Ud){ // only if Ud and Uq set - // _sqrt is an approx of sqrt (3-4% error) - Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); - }else{// only Uq available - no need for atan2 and sqrt - Uout = Uq / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + _PI_2); - } - // find the sector we are in currently - sector = floor(angle_el / _PI_3) + 1; - // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; - // two versions possible - float T0 = 0; // pulled to 0 - better for low power supply voltage - if (modulation_centered) { - T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 - } - - // calculate the duty cycles(times) - float Ta,Tb,Tc; - switch(sector){ - case 1: - Ta = T1 + T2 + T0/2; - Tb = T2 + T0/2; - Tc = T0/2; - break; - case 2: - Ta = T1 + T0/2; - Tb = T1 + T2 + T0/2; - Tc = T0/2; - break; - case 3: - Ta = T0/2; - Tb = T1 + T2 + T0/2; - Tc = T2 + T0/2; - break; - case 4: - Ta = T0/2; - Tb = T1+ T0/2; - Tc = T1 + T2 + T0/2; - break; - case 5: - Ta = T2 + T0/2; - Tb = T0/2; - Tc = T1 + T2 + T0/2; - break; - case 6: - Ta = T1 + T2 + T0/2; - Tb = T0/2; - Tc = T1 + T0/2; - break; - default: - // possible error state - Ta = 0; - Tb = 0; - Tc = 0; - } - - // calculate the phase voltages and center - Ua = Ta*driver->voltage_limit; - Ub = Tb*driver->voltage_limit; - Uc = Tc*driver->voltage_limit; - break; - } // set the voltages in driver From c99cc6da8f2e2adc97e4b8d1b3897263de09762f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Sep 2023 23:26:17 +0200 Subject: [PATCH 057/220] update README --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 47d7540f..729c23c6 100644 --- a/README.md +++ b/README.md @@ -30,8 +30,9 @@ Therefore this is an attempt to: > - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) > - Improved default trig functions (sine, cosine) - faster, smaller > - Overridable trig functions - plug in your own optimized versions -> - bugfix: microseconds overflow in velocity mode -> - bugfix: KV initialization +> - Bugfix: microseconds overflow in velocity mode +> - Bugfix: KV initialization +> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040 ## Arduino *SimpleFOClibrary* v2.3.1 From fb887073fe08c151eee183505778e0ff063681db Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Sep 2023 23:26:51 +0200 Subject: [PATCH 058/220] renesas small fixes --- src/drivers/hardware_specific/renesas/renesas.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index 4a6a3e92..f90a4c56 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -13,6 +13,8 @@ #include "communication/SimpleFOCDebug.h" #include "FspTimer.h" +#define GPT_OPEN (0x00475054ULL) + /* We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit) Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary. @@ -202,6 +204,7 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool t->timer_cfg.p_context = nullptr; t->timer_cfg.p_extend = &(t->ext_cfg); t->timer_cfg.cycle_end_ipl = BSP_IRQ_DISABLED; + t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR; t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg); t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED; @@ -256,6 +259,14 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; } + // lets stop the timer in case its running + if (GPT_OPEN == t->ctrl.open) { + if (R_GPT_Stop(&(t->ctrl)) != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: timer stop failed"); + return false; + } + } + memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { @@ -294,7 +305,7 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { uint32_t mask = 0; for (int i = 0; i < num_channels; i++) { - RenesasTimerConfig* t = params->timer_config[i]; + // RenesasTimerConfig* t = params->timer_config[i]; // if (R_GPT_Start(&(t->ctrl)) != FSP_SUCCESS) { // SIMPLEFOC_DEBUG("DRV: timer start failed"); // return false; From 0182f2fa4e4821dd62dd006bac9fa8ce5cfc7923 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 21 Sep 2023 19:50:01 +0200 Subject: [PATCH 059/220] update README for release --- README.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index c464a593..747b0f34 100644 --- a/README.md +++ b/README.md @@ -25,13 +25,16 @@ Therefore this is an attempt to: - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.1 -> - Support for Arduino UNO R4 Minima and UNO R4 WiFi (Renesas R7FA4M1 MCUs) +> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported) > - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells)) > - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) > - Improved default trig functions (sine, cosine) - faster, smaller > - Overridable trig functions - plug in your own optimized versions -> - Bugfix: microseconds overflow in velocity mode -> - Bugfix: KV initialization +> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287) +> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627)) +> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release) +> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28)) +> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688)) > - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040 ## Arduino *SimpleFOClibrary* v2.3.1 From e9678eb10f9d8302579169cfbf5d40cbb51605cc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 22 Sep 2023 23:40:59 +0200 Subject: [PATCH 060/220] add lint to workflow --- .github/workflows/ccpp.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 59ebbcf2..5c50bb1e 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -1,6 +1,14 @@ name: Library Compile on: [push, pull_request] jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library build: name: Test compiling runs-on: ubuntu-latest From 9be481e71223cb27aba7847e1b257fe05fea1d48 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 09:51:00 +0200 Subject: [PATCH 061/220] bump version number in library file --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 401523ba..69043875 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.0 +version=2.3.1 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 9c9879b2a7500bfa36f6160f986ac1c87969e606 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 10:35:17 +0200 Subject: [PATCH 062/220] change sketch/folder names to fix linting probs --- .../hall_sensor/{ => velocity_control}/velocity_control.ino | 0 ...ts_example.ino => hall_sensor_software_interrupts_example.ino} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename examples/motion_control/velocity_motion_control/hall_sensor/{ => velocity_control}/velocity_control.ino (100%) rename examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/{hall_sensors_software_interrupts_example.ino => hall_sensor_software_interrupts_example.ino} (100%) diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino similarity index 100% rename from examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino rename to examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensor_software_interrupts_example.ino similarity index 100% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensor_software_interrupts_example.ino From 5ecb58b2bd09ccfd045900a35eefe664c725bdcf Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 20:12:51 +0200 Subject: [PATCH 063/220] prepare README and version for next release --- README.md | 19 +++++-------------- library.properties | 2 +- 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 747b0f34..756bf854 100644 --- a/README.md +++ b/README.md @@ -24,20 +24,11 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.1 -> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported) -> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells)) -> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) -> - Improved default trig functions (sine, cosine) - faster, smaller -> - Overridable trig functions - plug in your own optimized versions -> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287) -> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627)) -> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release) -> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28)) -> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688)) -> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040 - -## Arduino *SimpleFOClibrary* v2.3.1 +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 +> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1) +> - And more bugfixes - see the [complete list of 2.3.2 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.2_Release) + +## Arduino *SimpleFOClibrary* v2.3.2

diff --git a/library.properties b/library.properties index 69043875..7242d57c 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.1 +version=2.3.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 87279080e2b2d5aa25149e90df65775244eb5204 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 17:59:32 +0200 Subject: [PATCH 064/220] refactoring of FOC --- src/common/base_classes/CurrentSense.cpp | 67 +++++++++++------------- src/common/base_classes/CurrentSense.h | 13 ++++- src/common/foc_utils.h | 6 +++ 3 files changed, 47 insertions(+), 39 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 609162c2..bbe5edc7 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -7,33 +7,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); - // currnet sign - if motor angle not provided the magnitude is always positive - float sign = 1; - + // calculate clarke transform - float i_alpha, i_beta; - if(!current.c){ - // if only two measured currents - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ - // if only two measured currents - float a = -current.c - current.b; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ - // if only two measured currents - float b = -current.a - current.c; - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; - }else{ - // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. - float mid = (1.f/3) * (current.a + current.b + current.c); - float a = current.a - mid; - float b = current.b - mid; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * b; - } + ABCurrent_s ABcurrent = getABCurrents(current); + + // current sign - if motor angle not provided the magnitude is always positive + float sign = 1; // if motor angle provided function returns signed value of the current // determine the sign of the current @@ -42,20 +21,38 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ float ct; float st; _sincos(motor_electrical_angle, &st, &ct); - sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1; } // return current magnitude - return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); + return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } // function used with the foc algorihtm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents -// - using getPhaseCurrents internally +// - using getPhaseCurrents and getABCurrents internally DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); + // calculate clarke transform + ABCurrent_s ABcurrent = getABCurrents(current); + + // calculate park transform + float ct; + float st; + _sincos(angle_el, &st, &ct); + DQCurrent_s return_current; + return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; + return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + return return_current; +} + +// function used with the foc algorihtm +// calculating Alpha Beta currents from phase currents +// - function calculating Clarke transform of the phase currents +ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -81,14 +78,10 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } - // calculate park transform - float ct; - float st; - _sincos(angle_el, &st, &ct); - DQCurrent_s return_current; - return_current.d = i_alpha * ct + i_beta * st; - return_current.q = i_beta * ct - i_alpha * st; - return return_current; + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = i_alpha; + return_ABcurrent.beta = i_beta; + return return_ABcurrent; } /** diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad9f926d..5292988b 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -53,7 +53,7 @@ class CurrentSense{ virtual PhaseCurrent_s getPhaseCurrents() = 0; /** * Function reading the magnitude of the current set to the motor - * It returns the abosolute or signed magnitude if possible + * It returns the absolute or signed magnitude if possible * It can receive the motor electrical angle to help with calculation * This function is used with the current control (not foc) * @@ -62,13 +62,22 @@ class CurrentSense{ virtual float getDCCurrent(float angle_el = 0); /** - * Function used for FOC contorl, it reads the DQ currents of the motor + * Function used for FOC control, it reads the DQ currents of the motor * It uses the function getPhaseCurrents internally * * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); + /** + * Function used for Clarke transform in FOC control + * It reads the phase currents of the motor + * It returns the alpha and beta currents + * + * @param current - phase current + */ + ABCurrent_s getABCurrents(PhaseCurrent_s current); + }; diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 0efe3b59..ac1ae458 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -56,6 +56,12 @@ struct DQVoltage_s float d; float q; }; +// alpha beta current structure +struct ABCurrent_s +{ + float alpha; + float beta; +}; /** From 7374f7a4f3297f51a2d67123cefd10d9bf7a2b39 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 23:35:48 +0200 Subject: [PATCH 065/220] Add getDQCurrents function for Park transform --- src/common/base_classes/CurrentSense.cpp | 22 ++++++++++++++++------ src/common/base_classes/CurrentSense.h | 9 +++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index bbe5edc7..c9394ad0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -39,12 +39,8 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ ABCurrent_s ABcurrent = getABCurrents(current); // calculate park transform - float ct; - float st; - _sincos(angle_el, &st, &ct); - DQCurrent_s return_current; - return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; - return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el); + return return_current; } @@ -84,6 +80,20 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } +// function used with the foc algorihtm +// calculating D and Q currents from Alpha Beta currents and electrical angle +// - function calculating Clarke transform of the phase currents +DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ + // calculate park transform + float ct; + float st; + _sincos(angle_el, &st, &ct); + DQCurrent_s return_current; + return_current.d = current.alpha * ct + current.beta * st; + return_current.q = current.beta * ct - current.alpha * st; + return return_current; +} + /** Driver linking to the current sense */ diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 5292988b..d9cd8c56 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -78,6 +78,15 @@ class CurrentSense{ */ ABCurrent_s getABCurrents(PhaseCurrent_s current); + /** + * Function used for Park transform in FOC control + * It reads the Alpha Beta currents and electircal angle of the motor + * It returns the D and Q currents + * + * @param current - phase current + */ + DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + }; From a373714ed1f21d864f8438a0534d7bb13d9fcce5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 26 Sep 2023 09:04:37 +0200 Subject: [PATCH 066/220] fix initialization error for StepperMotors --- src/StepperMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 491a20ee..a22496ba 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -134,7 +134,7 @@ int StepperMotor::alignSensor() { SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction - if(!_isset(sensor_direction)){ + if(sensor_direction == Direction::UNKNOWN){ // check if sensor needs zero search if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); // stop init if not found index From c83ea8a8c9c6f3b6b6d4336f083ec2e76420f03e Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 27 Sep 2023 09:41:20 +0200 Subject: [PATCH 067/220] bugfix for teensy 3.0-3.0, wrong check for timer 3 --- src/drivers/hardware_specific/teensy/teensy3_mcu.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp index 3a7b5de5..fe145273 100644 --- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -3,7 +3,6 @@ // if defined // - Teensy 3.0 MK20DX128 // - Teensy 3.1/3.2 MK20DX256 -// - Teensy 3.5 MK20DX128 // - Teensy LC MKL26Z64 // - Teensy 3.5 MK64FX512 // - Teensy 3.6 MK66FX1M0 @@ -14,7 +13,7 @@ #pragma message("SimpleFOC: compiling for Teensy 3.x") #pragma message("") -// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c +// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c#L627 #if defined(__MK20DX128__) #define FTM0_CH0_PIN 22 #define FTM0_CH1_PIN 23 @@ -116,7 +115,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i } } - #ifdef FTM3_SC // if the board has FTM3 timer +#ifdef FTM3_CH0_PIN // if the board has FTM3 timer if((Ah == FTM3_CH0_PIN && Al == FTM3_CH1_PIN) || (Ah == FTM3_CH2_PIN && Al == FTM3_CH3_PIN) || (Ah == FTM3_CH4_PIN && Al == FTM3_CH5_PIN) ){ @@ -134,7 +133,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i } } } - #endif +#endif #ifdef SIMPLEFOC_TEENSY_DEBUG SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Pins not on timers FTM0 or FTM3!"); From 27241b8d5b333a2e951308341560cae76f2e03fe Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:19:19 +0200 Subject: [PATCH 068/220] Create teensy4 --- .github/workflows/teensy4 | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/workflows/teensy4 diff --git a/.github/workflows/teensy4 b/.github/workflows/teensy4 new file mode 100644 index 00000000..b8079480 --- /dev/null +++ b/.github/workflows/teensy4 @@ -0,0 +1,31 @@ +name: PlatformIO CI + +on: [push] + +jobs: + build: + + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-latest, macos-latest, windows-latest] + example: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino,examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino|] + + steps: + - uses: actions/checkout@v3 + - uses: actions/cache@v3 + with: + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio + - uses: actions/setup-python@v4 + with: + python-version: '3.9' + - name: Install PlatformIO Core + run: pip install --upgrade platformio + +- name: Run PlatformIO + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: ${{ matrix.example }} From 3d31dc38ce9eb6d0f3e12c9f740bb9a78910b73f Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:20:26 +0200 Subject: [PATCH 069/220] Rename teensy4 to teensy4.yml --- .github/workflows/{teensy4 => teensy4.yml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/workflows/{teensy4 => teensy4.yml} (100%) diff --git a/.github/workflows/teensy4 b/.github/workflows/teensy4.yml similarity index 100% rename from .github/workflows/teensy4 rename to .github/workflows/teensy4.yml From b8301d5e6369035c9e528e6062b48a0b3cc09763 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:21:04 +0200 Subject: [PATCH 070/220] Update teensy4.yml --- .github/workflows/teensy4.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/teensy4.yml b/.github/workflows/teensy4.yml index b8079480..b8100fec 100644 --- a/.github/workflows/teensy4.yml +++ b/.github/workflows/teensy4.yml @@ -25,7 +25,7 @@ jobs: - name: Install PlatformIO Core run: pip install --upgrade platformio -- name: Run PlatformIO + - name: Run PlatformIO run: pio ci --lib="." --board=teensy41 --board=teensy40 env: PLATFORMIO_CI_SRC: ${{ matrix.example }} From 3a10d07996a7a09a8e49f281184e6a7ce5c8edbd Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:23:00 +0200 Subject: [PATCH 071/220] Update teensy4.yml --- .github/workflows/teensy4.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/teensy4.yml b/.github/workflows/teensy4.yml index b8100fec..c5c499bd 100644 --- a/.github/workflows/teensy4.yml +++ b/.github/workflows/teensy4.yml @@ -9,7 +9,7 @@ jobs: strategy: matrix: os: [ubuntu-latest, macos-latest, windows-latest] - example: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino,examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino|] + example: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] steps: - uses: actions/checkout@v3 From f0ea6ccc813049cee013c36aee3c3f03d5b60991 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:30:25 +0200 Subject: [PATCH 072/220] Update and rename teensy4.yml to teensy.yml --- .github/workflows/teensy.yml | 37 +++++++++++++++++++++++++++++++++++ .github/workflows/teensy4.yml | 31 ----------------------------- 2 files changed, 37 insertions(+), 31 deletions(-) create mode 100644 .github/workflows/teensy.yml delete mode 100644 .github/workflows/teensy4.yml diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml new file mode 100644 index 00000000..b8cb0c43 --- /dev/null +++ b/.github/workflows/teensy.yml @@ -0,0 +1,37 @@ +name: PlatformIO - Teensy build + +on: [push] + +jobs: + build: + + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-latest] + example_4: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] + example_3: [examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] + + steps: + - uses: actions/checkout@v3 + - uses: actions/cache@v3 + with: + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio + - uses: actions/setup-python@v4 + with: + python-version: '3.9' + - name: Install PlatformIO Core + run: pip install --upgrade platformio + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: ${{ matrix.example_4 }} + + - name: PIO Run Teensy 3 + run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 + env: + PLATFORMIO_CI_SRC: ${{ matrix.example_3 }} diff --git a/.github/workflows/teensy4.yml b/.github/workflows/teensy4.yml deleted file mode 100644 index c5c499bd..00000000 --- a/.github/workflows/teensy4.yml +++ /dev/null @@ -1,31 +0,0 @@ -name: PlatformIO CI - -on: [push] - -jobs: - build: - - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-latest, macos-latest, windows-latest] - example: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] - - steps: - - uses: actions/checkout@v3 - - uses: actions/cache@v3 - with: - path: | - ~/.cache/pip - ~/.platformio/.cache - key: ${{ runner.os }}-pio - - uses: actions/setup-python@v4 - with: - python-version: '3.9' - - name: Install PlatformIO Core - run: pip install --upgrade platformio - - - name: Run PlatformIO - run: pio ci --lib="." --board=teensy41 --board=teensy40 - env: - PLATFORMIO_CI_SRC: ${{ matrix.example }} From 2c039baf3c94b0ad4efa9b3a480aaf741d0a9988 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:36:30 +0200 Subject: [PATCH 073/220] Update teensy.yml --- .github/workflows/teensy.yml | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index b8cb0c43..9fc88b9a 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -5,13 +5,7 @@ on: [push] jobs: build: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-latest] - example_4: [examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] - example_3: [examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino, examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino] - + runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - uses: actions/cache@v3 @@ -29,9 +23,19 @@ jobs: - name: PIO Run Teensy 4 run: pio ci --lib="." --board=teensy41 --board=teensy40 env: - PLATFORMIO_CI_SRC: ${{ matrix.example_4 }} + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino - name: PIO Run Teensy 3 run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 env: - PLATFORMIO_CI_SRC: ${{ matrix.example_3 }} + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino + + - name: PIO Run Teensy 3 + run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino From c42ebc414875e00144d5c0197e3aedc7f4a513d0 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 27 Sep 2023 10:43:53 +0200 Subject: [PATCH 074/220] added pio flag to the readme --- README.md | 3 ++- src/BLDCMotor.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 747b0f34..a8e59c0b 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,8 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) +![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) + ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index de76ce0f..77e4c788 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -561,6 +561,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { center = driver->voltage_limit/2; if (foc_modulation == FOCModulationType::SpaceVectorPWM){ + // discussed here: https://community.simplefoc.com/t/embedded-world-2023-stm32-cordic-co-processor/3107/165?u=candas1 + // a bit more info here: https://microchipdeveloper.com/mct5001:which-zsm-is-best // Midpoint Clamp float Umin = min(Ua, min(Ub, Uc)); float Umax = max(Ua, max(Ub, Uc)); From 191e13db336e1a60273e253244b7607d96150c02 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 27 Sep 2023 10:47:03 +0200 Subject: [PATCH 075/220] radme updated with the changes --- README.md | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index a8e59c0b..a1936c74 100644 --- a/README.md +++ b/README.md @@ -25,20 +25,11 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.1 -> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported) -> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells)) -> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) -> - Improved default trig functions (sine, cosine) - faster, smaller -> - Overridable trig functions - plug in your own optimized versions -> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287) -> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627)) -> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release) -> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28)) -> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688)) -> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040 - -## Arduino *SimpleFOClibrary* v2.3.1 +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 +> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321) +> - Added teensy3/4 compile to the github CI using platformio + +## Arduino *SimpleFOClibrary* v2.3.2

From 7009b4812c5c43967df9ebdcf971566e27ce235d Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 27 Sep 2023 11:41:19 +0200 Subject: [PATCH 076/220] keyword update with verbose --- keywords.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/keywords.txt b/keywords.txt index 1f200e52..d8fd3e73 100644 --- a/keywords.txt +++ b/keywords.txt @@ -133,6 +133,7 @@ sensor_direction KEYWORD2 sensor_offset KEYWORD2 zero_electric_angle KEYWORD2 verbose KEYWORD2 +verboseMode KEYWORD1 decimal_places KEYWORD2 phase_resistance KEYWORD2 phase_inductance KEYWORD2 From 80dea9152e2d072d9d3e59e92df346d7a6907cd6 Mon Sep 17 00:00:00 2001 From: Martino Facchin Date: Wed, 27 Sep 2023 15:44:02 +0200 Subject: [PATCH 077/220] renesas: disable capture interrupt before open() --- src/drivers/hardware_specific/renesas/renesas.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index f90a4c56..02299f11 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -207,6 +207,10 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR; t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg); + t->ext_cfg.capture_a_ipl = BSP_IRQ_DISABLED; + t->ext_cfg.capture_a_irq = FSP_INVALID_VECTOR; + t->ext_cfg.capture_b_ipl = BSP_IRQ_DISABLED; + t->ext_cfg.capture_b_irq = FSP_INVALID_VECTOR; t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED; t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR; t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0; From 7499e3032e74426a9715df3e42e94290d08250a5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 27 Sep 2023 21:10:18 +0200 Subject: [PATCH 078/220] renesas driver improvements --- src/drivers/hardware_specific/renesas/renesas.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp index f90a4c56..ff0ebb80 100644 --- a/src/drivers/hardware_specific/renesas/renesas.cpp +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -144,7 +144,7 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]); // check its not used if (channel_used[timer_channel]) { - SIMPLEFOC_DEBUG("DRV: channel in use"); + SIMPLEFOC_DEBUG("DRV: channel in use: ", timer_channel); return false; } @@ -258,26 +258,17 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; } - - // lets stop the timer in case its running - if (GPT_OPEN == t->ctrl.open) { - if (R_GPT_Stop(&(t->ctrl)) != FSP_SUCCESS) { - SIMPLEFOC_DEBUG("DRV: timer stop failed"); - return false; - } - } - memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { SIMPLEFOC_DEBUG("DRV: open failed"); return false; } - #if defined(SIMPLEFOC_RESENSAS_DEBUG) if (err == FSP_ERR_ALREADY_OPEN) { SimpleFOCDebug::println("DRV: timer already open"); + return false; } - #endif + err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts); if (err != FSP_SUCCESS) { SIMPLEFOC_DEBUG("DRV: period set failed"); From cc82e8818b8b195f886995e8eab7b989a1b81029 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Fri, 29 Sep 2023 20:28:16 +0200 Subject: [PATCH 079/220] ADC calibration --- .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 3e10bbca..079b43ac 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -110,6 +110,9 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p MX_ADC1_Init(&hadc1); MX_ADC2_Init(&hadc2); + //HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED); + //HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); + MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); From e28e34d686509a08aa1e9253a83e190f045f33be Mon Sep 17 00:00:00 2001 From: Candas1 Date: Fri, 29 Sep 2023 20:28:50 +0200 Subject: [PATCH 080/220] ADC Calibration --- .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 079b43ac..8456759c 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -110,8 +110,8 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p MX_ADC1_Init(&hadc1); MX_ADC2_Init(&hadc2); - //HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED); - //HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); From 655c099a0bfd679ab8249ffb684fc86093c7183d Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sat, 21 Oct 2023 15:58:00 +0200 Subject: [PATCH 081/220] Move Ualpha and Ubeta to FOCMotor --- src/BLDCMotor.h | 3 +-- src/common/base_classes/FOCMotor.h | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a1f196ab..a7155c1f 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -69,8 +69,7 @@ class BLDCMotor: public FOCMotor void move(float target = NOT_SET) override; float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 318be99a..d4aa152d 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -161,6 +161,8 @@ class FOCMotor DQVoltage_s voltage;//!< current d and q voltage set to the motor DQCurrent_s current;//!< current d and q current measured float voltage_bemf; //!< estimated backemf voltage (if provided KV constant) + float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter From deb7ff75d5bb4edc3992999a184c57b509b1d1a7 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Mon, 23 Oct 2023 22:40:01 +0200 Subject: [PATCH 082/220] Adc calibration --- .../hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 8daa7eef..51b494f4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -66,6 +66,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle); + // start the adc HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); From b780a451b2a2361f7016b1494eb4f0551c8a2159 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Mon, 23 Oct 2023 23:07:58 +0200 Subject: [PATCH 083/220] Faster atan2 --- src/common/foc_utils.cpp | 27 +++++++++++++++++++++++++++ src/common/foc_utils.h | 5 +++++ 2 files changed, 32 insertions(+) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 4cb09863..9136dd73 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -44,6 +44,33 @@ __attribute__((weak)) void _sincos(float a, float* s, float* c){ *c = _cos(a); } +// fast_atan2 based on https://math.stackexchange.com/a/1105038/81278 +// Via Odrive project +// https://github.com/odriverobotics/ODrive/blob/master/Firmware/MotorControl/utils.cpp +// This function is MIT licenced, copyright Oskar Weigl/Odrive Robotics +// The origin for Odrive atan2 is public domain. Thanks to Odrive for making +// it easy to borrow. +__attribute__((weak)) float _atan2(float y, float x) { + // a := min (|x|, |y|) / max (|x|, |y|) + float abs_y = fabsf(y); + float abs_x = fabsf(x); + // inject FLT_MIN in denominator to avoid division by zero + float a = min(abs_x, abs_y) / (max(abs_x, abs_y)); + // s := a * a + float s = a * a; + // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a + float r = + ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; + // if |y| > |x| then r := 1.57079637 - r + if (abs_y > abs_x) r = 1.57079637f - r; + // if x < 0 then r := 3.14159274 - r + if (x < 0.0f) r = 3.14159274f - r; + // if y < 0 then r := -r + if (y < 0.0f) r = -r; + + return r; + } + // normalizing radian angle to [0,2PI] __attribute__((weak)) float _normalizeAngle(float angle){ diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 0efe3b59..abdeebf8 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -79,6 +79,11 @@ float _cos(float a); */ void _sincos(float a, float* s, float* c); +/** + * Function approximating atan2 + * + */ +float _atan2(float y, float x); /** * normalizing radian angle to [0,2PI] From fb9f57ac9b1efd9611004dfc3663c110b93d0c1e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 20:38:38 +0100 Subject: [PATCH 084/220] remove deprecated header --- src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 807c387d..6581de87 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -6,7 +6,7 @@ #include "freertos/task.h" #include "rom/ets_sys.h" #include "esp_attr.h" -#include "esp_intr.h" +//#include "esp_intr.h" deprecated #include "soc/rtc_io_reg.h" #include "soc/rtc_cntl_reg.h" #include "soc/sens_reg.h" From e28f0cde02318c0b6b030c1ad238b3de4cb0ea9d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 21:05:14 +0100 Subject: [PATCH 085/220] change cast to make ESP32 compiler happy --- src/common/foc_utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 9136dd73..071e70cd 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -92,9 +92,9 @@ __attribute__((weak)) float _sqrtApprox(float number) {//low in fat // x = number * 0.5F; float y = number; - long i = * ( long * ) &y; + uint32_t i = *reinterpret_cast(&y); i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = *reinterpret_cast(&i); // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } From 9b3037b2f8e6221f9374466f190a35217b291c81 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 21:12:05 +0100 Subject: [PATCH 086/220] fix missing extension in workflow file --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5c50bb1e..1ec45f72 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -59,7 +59,7 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json - sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone + sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json From 3dd61bbf0f145a30fc4c78aa47490859daa7a76b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 21:16:01 +0100 Subject: [PATCH 087/220] deconfuse esp32 compiler --- src/common/foc_utils.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 071e70cd..e29a0901 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -91,7 +91,8 @@ __attribute__((weak)) float _sqrtApprox(float number) {//low in fat // const float f = 1.5F; // better precision // x = number * 0.5F; - float y = number; + float y; + y = number; uint32_t i = *reinterpret_cast(&y); i = 0x5f375a86 - ( i >> 1 ); y = *reinterpret_cast(&i); From 839e293ff7892d3f764387b2d0c4cad2ca4c2ce4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 21:31:02 +0100 Subject: [PATCH 088/220] trying to fix sqrt compiler issue on esp32 --- src/common/foc_utils.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index e29a0901..233bd246 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -87,15 +87,10 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root __attribute__((weak)) float _sqrtApprox(float number) {//low in fat - // float x; - // const float f = 1.5F; // better precision - - // x = number * 0.5F; - float y; - y = number; - uint32_t i = *reinterpret_cast(&y); - i = 0x5f375a86 - ( i >> 1 ); - y = *reinterpret_cast(&i); - // y = y * ( f - ( x * y * y ) ); // better precision - return number * y; + union { + float f; + uint32_t i; + } y = { .f = number }; + y.i = 0x5f375a86 - ( y.i >> 1 ); + return number * y.f; } From bfa492c3e0c27912e6fab1222b5f850a836bee41 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Mon, 30 Oct 2023 21:57:14 +0100 Subject: [PATCH 089/220] Initialize Ualpha and Ubeta --- src/StepperMotor.h | 2 -- src/common/base_classes/FOCMotor.cpp | 4 ++++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7eda3167..45d20c63 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -73,8 +73,6 @@ class StepperMotor: public FOCMotor */ void move(float target = NOT_SET) override; - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..c78db959 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -32,6 +32,10 @@ FOCMotor::FOCMotor() // voltage bemf voltage_bemf = 0; + + // Initialize phase voltages U alpha and U beta used for inverse Park and Clarke transform + Ualpha = 0; + Ubeta = 0; //monitor_port monitor_port = nullptr; From c39ea37addc3c2811cb9c8021e0d82fcce3d5c4f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 22:28:24 +0100 Subject: [PATCH 090/220] fix esp32 compiler warnings --- src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp | 1 + src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 6581de87..9cd1b34e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -7,6 +7,7 @@ #include "rom/ets_sys.h" #include "esp_attr.h" //#include "esp_intr.h" deprecated +#include "esp_intr_alloc.h" #include "soc/rtc_io_reg.h" #include "soc/rtc_cntl_reg.h" #include "soc/sens_reg.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 3df9dff6..2057463c 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -121,6 +121,8 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler } +static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); + // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm0_isr_handler(void*){ // // high side @@ -139,6 +141,7 @@ static void IRAM_ATTR mcpwm0_isr_handler(void*){ // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; } +static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm1_isr_handler(void*){ From f39aa92a762695d8b577ca8965762095d92b1cc6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 30 Oct 2023 23:38:10 +0100 Subject: [PATCH 091/220] don't use dev platform for esp32 tests --- .github/workflows/ccpp.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 1ec45f72..6c396ea9 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -58,15 +58,15 @@ jobs: sketch-names: single_full_control_example.ino - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 - platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples From 742924c46683ee3a4539d5a6d9d85d8c714a2143 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 31 Oct 2023 17:13:43 +0100 Subject: [PATCH 092/220] fix #324 to enable more than one HallSensor --- src/sensors/HallSensor.cpp | 1 - src/sensors/HallSensor.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 38767d59..d9e5ec83 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -53,7 +53,6 @@ void HallSensor::updateState() { hall_state = new_hall_state; int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; - static Direction old_direction; if (new_electric_sector - electric_sector > 3) { //underflow direction = Direction::CCW; diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index ad50c7d5..78e1b416 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -62,6 +62,7 @@ class HallSensor: public Sensor{ // whether last step was CW (+1) or CCW (-1). Direction direction; + Direction old_direction; void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); From 2b18cac1d55dabe3016c33166f5db72369586799 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 31 Oct 2023 17:20:48 +0100 Subject: [PATCH 093/220] fix StepDirListener PinStatus type problem --- src/communication/StepDirListener.h | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index f9691fd3..3627b5e7 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,11 +5,6 @@ #include "../common/foc_utils.h" -#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR) -#define PinStatus int -#endif - - /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -53,7 +48,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 + decltype(RISING) polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable From 7b336cfc7f6d7a2539539bd1d0f0b1e23351298e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 8 Nov 2023 00:18:55 +0100 Subject: [PATCH 094/220] fix const char warnings --- src/communication/Commander.cpp | 4 ++-- src/communication/Commander.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index ee6be9f9..d2822b04 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -11,10 +11,10 @@ Commander::Commander(char eol, bool echo){ } -void Commander::add(char id, CommandCallback onCommand, char* label ){ +void Commander::add(char id, CommandCallback onCommand, const char* label ){ call_list[call_count] = onCommand; call_ids[call_count] = id; - call_label[call_count] = label; + call_label[call_count] = (char*)label; call_count++; } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 91e3dc45..4ec2b281 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -91,7 +91,7 @@ class Commander * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent */ - void add(char id , CommandCallback onCommand, char* label = nullptr); + void add(char id , CommandCallback onCommand, const char* label = nullptr); // printing variables VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text From 9fd66216c100a76adc013686c07e2dc26a140918 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 8 Nov 2023 00:19:26 +0100 Subject: [PATCH 095/220] check returned sensor angle is non-negative --- src/common/base_classes/Sensor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index a80fa438..b0a8f4db 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -6,6 +6,8 @@ void Sensor::update() { float val = getSensorAngle(); + if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors. + return; // TODO signal error, e.g. via a flag and counter angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation From be1f0aa8baa737ae85a40ef7e6b0f1b5238ac124 Mon Sep 17 00:00:00 2001 From: Raffael Poeggel Date: Sun, 12 Nov 2023 20:48:25 +0100 Subject: [PATCH 096/220] [FIX] 2 phase measurements if-else statement bug --- src/common/base_classes/CurrentSense.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 609162c2..217e8a67 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -16,12 +16,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ + }else if(!current.a){ // if only two measured currents float a = -current.c - current.b; i_alpha = a; i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; @@ -62,12 +62,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ + }else if(!current.a){ // if only two measured currents float a = -current.c - current.b; i_alpha = a; i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; From 716ad1dbddf13bff928a535cf861e1becdd027ec Mon Sep 17 00:00:00 2001 From: Candas1 Date: Thu, 16 Nov 2023 09:13:25 +0000 Subject: [PATCH 097/220] ADC Calibration for g4 and l4 --- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 4 ++++ .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index ae96bad7..35689d8f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -61,6 +61,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + // start the adc HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); // restart all the timers of the driver diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index edac6414..ed229606 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -61,6 +61,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + // start the adc HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); // restart all the timers of the driver From 5c724d9aedc555b4291fb49107f603b6c8795280 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Thu, 16 Nov 2023 10:09:35 +0000 Subject: [PATCH 098/220] build flag for use adc interrupt on f1/f4/g4/l4 --- .../stm32/stm32f1/stm32f1_hal.cpp | 4 ++++ .../stm32/stm32f1/stm32f1_mcu.cpp | 18 ++++++++++++++---- .../stm32/stm32f4/stm32f4_hal.cpp | 4 ++++ .../stm32/stm32f4/stm32f4_mcu.cpp | 19 +++++++++++++++---- .../stm32/stm32g4/stm32g4_hal.cpp | 5 ++++- .../stm32/stm32g4/stm32g4_mcu.cpp | 17 ++++++++++++++--- .../stm32/stm32l4/stm32l4_hal.cpp | 5 ++++- .../stm32/stm32l4/stm32l4_mcu.cpp | 19 +++++++++++++++---- 8 files changed, 74 insertions(+), 17 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..ec75ef4f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -124,9 +124,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + #endif cs_params->adc_handle = &hadc; @@ -151,6 +153,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -158,5 +161,6 @@ extern "C" { } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 51b494f4..3a97dc54 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -69,9 +69,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); - + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -81,13 +85,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -104,5 +113,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index 68a9d094..dcf32137 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,9 +135,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif cs_params->adc_handle = &hadc; return 0; @@ -160,11 +162,13 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 60344781..3a7b131c 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -58,8 +58,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -69,13 +74,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -92,5 +102,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 91322112..bf89463b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -181,7 +181,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT if(hadc.Instance == ADC1) { // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); @@ -214,6 +214,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC5_IRQn); } +#endif #endif cs_params->adc_handle = &hadc; @@ -237,6 +238,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -263,5 +265,6 @@ extern "C" { } #endif } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 35689d8f..4e1aa9ac 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -66,7 +66,12 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -75,13 +80,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -98,5 +108,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..688ba796 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -180,7 +180,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT if(hadc.Instance == ADC1) { // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); @@ -213,6 +213,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC5_IRQn); } +#endif #endif cs_params->adc_handle = &hadc; @@ -236,6 +237,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -262,5 +264,6 @@ extern "C" { } #endif } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index ed229606..d1ca2814 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -65,8 +65,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); - // start the adc + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -75,13 +80,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -98,5 +108,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file From 72cdd21f02d77a02e8d9e3198fc5a0ee41291f75 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 19 Nov 2023 19:03:42 +0100 Subject: [PATCH 099/220] Update CurrentSense.cpp Fix Clarke transform for dual shunt --- src/common/base_classes/CurrentSense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index c9394ad0..c57bdfe6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -55,12 +55,12 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ + }else if(!current.a){ // if only two measured currents float a = -current.c - current.b; i_alpha = a; i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; @@ -99,4 +99,4 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ */ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; -} \ No newline at end of file +} From 6adcf6e8fdc1705cb3ce1fd201a2d5fd752c25fe Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Mon, 27 Nov 2023 00:31:04 +0100 Subject: [PATCH 100/220] Fix ESP32 Lowside Current Sense ADC2 init Co-authored-by: @nmscode --- .../hardware_specific/esp32/esp32_adc_driver.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 9cd1b34e..57ffdfb5 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -154,9 +154,8 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ } if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); @@ -225,9 +224,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) __analogInit(); if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); @@ -257,4 +255,4 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) } -#endif \ No newline at end of file +#endif From d4e1c2cd04ed19552e9f488c3a3e3ccbfef89c21 Mon Sep 17 00:00:00 2001 From: nmscode <105442557+nmscode@users.noreply.github.com> Date: Mon, 27 Nov 2023 16:48:28 -0500 Subject: [PATCH 101/220] Remove sqrt 2 multiplication and correct bemf calculation --- src/BLDCMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8dc2f5cd..16534199 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -46,7 +46,7 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) // 1/sqrt(2) - rms value KV_rating = NOT_SET; if (_isset(_KV)) - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -396,7 +396,7 @@ void BLDCMotor::move(float new_target) { if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; From afdbd9dba8f4e31af71afa5e4c07b35a19f9b185 Mon Sep 17 00:00:00 2001 From: nmscode <105442557+nmscode@users.noreply.github.com> Date: Mon, 27 Nov 2023 16:52:13 -0500 Subject: [PATCH 102/220] apply fix to stepper motor logic as well --- src/StepperMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 7831f0bf..6cbe417a 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -16,7 +16,7 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/K_bemf // usually used rms - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -292,7 +292,7 @@ void StepperMotor::move(float new_target) { if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; From 9782d73a8d915e92da73376680ce924c61021c34 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 28 Nov 2023 22:55:11 +0100 Subject: [PATCH 103/220] add missing loopFOC to example --- .../hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 51ac990e..ab8e3bba 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -96,6 +96,7 @@ void setup() { void loop() { // main FOC algorithm function + motor.loopFOC(); // Motion control function motor.move(); From ecc7f1ab6a149998a6ed5fb02c27fb7c0407a084 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 28 Nov 2023 23:16:51 +0100 Subject: [PATCH 104/220] fix #345 --- .../encoder/full_control_serial/full_control_serial.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index a5b72f82..2b4cf6f1 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -29,7 +29,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); -BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE); +BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INL_A, INH_B,INL_B, INH_C,INL_C, EN_GATE); // encoder instance Encoder encoder = Encoder(2, 3, 8192); From 3eb2bc34cafffda3280e9f69e0183fe42802e384 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 1 Dec 2023 22:26:25 +0100 Subject: [PATCH 105/220] readme updates for 2.3.2 release --- README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index f41217e1..75475bec 100644 --- a/README.md +++ b/README.md @@ -27,9 +27,16 @@ Therefore this is an attempt to: > NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 > - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1) +> - Bugfix for stepper motor initialization +> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning > - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321) > - Added teensy3/4 compile to the github CI using platformio -> - And more bugfixes - see the [complete list of 2.3.2 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.2_Release) +> - Fix compile issues with recent versions of ESP32 framework +> - Add ADC calibration on STM32 MCUs +> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346) +> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322) +> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1) + ## Arduino *SimpleFOClibrary* v2.3.2 From 8ea87abfe350c09e02344c46cfd9f3fc9de432f8 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 2 Dec 2023 21:42:31 +0100 Subject: [PATCH 106/220] timeout configurable in MagneticSensorPWM #348 --- src/sensors/MagneticSensorPWM.cpp | 2 +- src/sensors/MagneticSensorPWM.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index cf211644..04b7cc75 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -85,7 +85,7 @@ float MagneticSensorPWM::getSensorAngle(){ int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse - pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable? + pulse_length_us = pulseIn(pinPWM, HIGH, timeout_us); // 1200us timeout, should this be configurable? } return pulse_length_us; } diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 48492c84..a5fd7e6e 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -44,6 +44,8 @@ class MagneticSensorPWM: public Sensor{ void enableInterrupt(void (*doPWM)()); unsigned long pulse_length_us; + unsigned int timeout_us = 1200; + private: // raw count (typically in range of 0-1023) int raw_count; From ef580d79ebc5ae7fb913584b2bb923e9df2b9775 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 17 Dec 2023 11:12:28 +0100 Subject: [PATCH 107/220] Prevent double constants --- src/common/base_classes/Sensor.cpp | 2 +- src/common/foc_utils.cpp | 2 +- src/current_sense/InlineCurrentSense.cpp | 12 ++++++------ src/current_sense/LowsideCurrentSense.cpp | 12 ++++++------ 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index b0a8f4db..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 233bd246..0809b137 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256 /_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 492b3ac9..c3db74ef 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -156,9 +156,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -203,9 +203,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index a706beeb..aeb8dea0 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -157,9 +157,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -204,9 +204,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); From b0d7daafbfb8589ed0d92795dd82d6c3f9ff3a85 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 11:13:34 +0100 Subject: [PATCH 108/220] fix double constants to be floats --- src/common/base_classes/Sensor.cpp | 2 +- src/common/defaults.h | 2 +- src/common/foc_utils.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index b0a8f4db..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; diff --git a/src/common/defaults.h b/src/common/defaults.h index c0a46182..ac57daa1 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,7 +43,7 @@ // align voltage #define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant +#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant // current sense default parameters #define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 233bd246..fa937d15 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { From 5f259f7944cc55fe389fffb72b62c3b58e69ec0f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 11:13:58 +0100 Subject: [PATCH 109/220] prepare files for next release version --- README.md | 13 ++----------- library.properties | 2 +- 2 files changed, 3 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 75475bec..7d8150d0 100644 --- a/README.md +++ b/README.md @@ -25,17 +25,8 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 -> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1) -> - Bugfix for stepper motor initialization -> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning -> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321) -> - Added teensy3/4 compile to the github CI using platformio -> - Fix compile issues with recent versions of ESP32 framework -> - Add ADC calibration on STM32 MCUs -> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346) -> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322) -> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1) +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 +> - And more bugfixes - see the complete list of 2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) ## Arduino *SimpleFOClibrary* v2.3.2 diff --git a/library.properties b/library.properties index 7242d57c..0a2606b2 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.2 +version=2.3.3 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From b7300fe356e854b58a67f2a25efe9c1a4230b35e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 22:39:11 +0100 Subject: [PATCH 110/220] Add library.json to set libArchive #357 --- library.json | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 library.json diff --git a/library.json b/library.json new file mode 100644 index 00000000..25e2a353 --- /dev/null +++ b/library.json @@ -0,0 +1,5 @@ +{ + "build": { + "libArchive": false + } +} From 4934aae508dff2c2fea1ce129cbc3a58ec5cd124 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 7 Jan 2024 21:22:49 +0100 Subject: [PATCH 111/220] add currentsense.enable/disable --- src/BLDCMotor.cpp | 9 +++++++++ src/common/base_classes/CurrentSense.cpp | 9 +++++++++ src/common/base_classes/CurrentSense.h | 10 ++++++++++ 3 files changed, 28 insertions(+) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 16534199..8fd54ba6 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -111,6 +111,8 @@ void BLDCMotor::init() { // disable motor driver void BLDCMotor::disable() { + // disable the current sense + if(current_sense) current_sense->disable(); // set zero to PWM driver->setPwm(0, 0, 0); // disable the driver @@ -125,6 +127,13 @@ void BLDCMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); + // enable the current sense + if(current_sense) current_sense->enable(); + // reset the pids + PID_velocity.reset(); + P_angle.reset(); + PID_current_q.reset(); + PID_current_d.reset(); // motor status update enabled = 1; } diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index c57bdfe6..b732c48e 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -100,3 +100,12 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; } + + +void enable(){ + // nothing is done here, but you can override this function +}; + +void disable(){ + // nothing is done here, but you can override this function +}; \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index d9cd8c56..1c839053 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -87,7 +87,17 @@ class CurrentSense{ */ DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + /** + * enable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void enable(); + /** + * disable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void disable(); }; #endif \ No newline at end of file From 9db53cdd17e6365bf6ab0e484659a636d9446578 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 9 Jan 2024 12:11:58 +0100 Subject: [PATCH 112/220] add methods to class correctly --- src/common/base_classes/CurrentSense.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index b732c48e..03ea19ea 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -102,10 +102,10 @@ void CurrentSense::linkDriver(BLDCDriver* _driver) { } -void enable(){ +void CurrentSense::enable(){ // nothing is done here, but you can override this function }; -void disable(){ +void CurrentSense::disable(){ // nothing is done here, but you can override this function }; \ No newline at end of file From 71ce4b1b95891eb5d9103a2c5f397be786176bdc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 9 Jan 2024 12:16:14 +0100 Subject: [PATCH 113/220] solve AVR compiler warnings --- .../hardware_specific/atmega/atmega2560_mcu.cpp | 12 ++++++++---- .../hardware_specific/atmega/atmega328_mcu.cpp | 12 ++++++++---- .../hardware_specific/atmega/atmega32u4_mcu.cpp | 12 ++++++++---- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 8a7fbbec..532b3ce3 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -57,7 +57,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -75,7 +76,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -94,7 +96,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; // _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 53fb1086..9df9b8a7 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -53,7 +53,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -70,7 +71,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -89,7 +91,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b4ad3101..4cf454ae 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -41,7 +41,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -56,7 +57,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -72,7 +74,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -119,7 +122,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } From 241e8c04dbd4ee8ea3476b166d6e0684b1cd08e1 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:25:13 -0600 Subject: [PATCH 114/220] LinearHall improvements and move to main repository Changes compared to the original pull request in the drivers repository https://github.com/simplefoc/Arduino-FOC-drivers/pull/12 1. Added a version of init which turns the motor one revolution to find the center values of the sensors. 2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance. 3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update. 4. Changed to use the much faster _atan2 function that was added to foc_utils recently. 5. Added examples. --- .../angle_control/angle_control.ino | 117 ++++++++++++++++++ .../voltage_control/voltage_control.ino | 96 ++++++++++++++ .../velocity_control/velocity_control.ino | 109 ++++++++++++++++ src/SimpleFOC.h | 1 + src/sensors/LinearHall.cpp | 109 ++++++++++++++++ src/sensors/LinearHall.h | 45 +++++++ 6 files changed, 477 insertions(+) create mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino create mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino create mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino create mode 100644 src/sensors/LinearHall.cpp create mode 100644 src/sensors/LinearHall.h diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino new file mode 100644 index 00000000..8179008d --- /dev/null +++ b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino @@ -0,0 +1,117 @@ +/** + * + * Position/angle motion control example + * Steps: + * 1) Configure the motor and hall sensor + * 2) Run the code + * 3) Set the target angle (in radians) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + // index search velocity [rad/s] + motor.velocity_index_search = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::angle; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle P controller + motor.P_angle.P = 20; + // maximal velocity of the position control + motor.velocity_limit = 4; + + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target angle"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_angle); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino new file mode 100644 index 00000000..e646849a --- /dev/null +++ b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino @@ -0,0 +1,96 @@ +/** + * + * Torque control example using voltage control loop. + * + * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers + * you a way to control motor torque by setting the voltage to the motor instead of the current. + * + * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + + +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // choose FOC modulation (optional) + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino new file mode 100644 index 00000000..870a0899 --- /dev/null +++ b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino @@ -0,0 +1,109 @@ +/** + * + * Velocity motion control example + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + _delay(1000); +} + + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_velocity); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..9e1c80c6 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,6 +104,7 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" +#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp new file mode 100644 index 00000000..36303aaa --- /dev/null +++ b/src/sensors/LinearHall.cpp @@ -0,0 +1,109 @@ +#include "LinearHall.h" + +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. +__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) +{ + *a = analogRead(hallA); + *b = analogRead(hallB); +} + +LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ + centerA = 512; + centerB = 512; + pinA = _hallA; + pinB = _hallB; + pp = _pp; +} + +float LinearHall::getSensorAngle() { + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + //offset readings using center values, then compute angle + float reading = _atan2(lastA - centerA, lastB - centerB); + + //handle rollover logic between each electrical revolution of the motor + if (reading > prev_reading) { + if (reading - prev_reading >= PI) { + if (electrical_rev - 1 < 0) { + electrical_rev = pp - 1; + } else { + electrical_rev = electrical_rev - 1; + } + } + } else if (reading < prev_reading) { + if (prev_reading - reading >= PI) { + if (electrical_rev + 1 >= pp) { + electrical_rev = 0; + } else { + electrical_rev = electrical_rev + 1; + } + } + } + + //convert result from electrical angle and electrical revolution count to shaft angle in radians + float result = (reading + PI) / _2PI; + result = _2PI * (result + electrical_rev) / pp; + + //update previous reading for rollover handling + prev_reading = reading; + return result; +} + +void LinearHall::init(int _centerA, int _centerB) { + // Skip configuring the pins here because they normally default to input anyway, and + // this makes it possible to use ADC channel numbers instead of pin numbers when using + // custom implementation of ReadLinearHalls, to avoid having to remap them every update. + // If pins do need to be configured, it can be done by user code before calling init. + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + centerA = _centerA; + centerB = _centerB; + + //establish initial reading for rollover handling + electrical_rev = 0; + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} + +void LinearHall::init(FOCMotor *motor) { + if (!motor->enabled) { + SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); + return; + } + + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + int minA, maxA, minB, maxB; + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + minA = maxA = centerA = lastA; + minB = maxB = centerB = lastB; + + // move one mechanical revolution forward + for (int i = 0; i <= 2000; i++) + { + float angle = _3PI_2 + _2PI * i * pp / 2000.0f; + motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + + if (lastA < minA) + minA = lastA; + if (lastA > maxA) + maxA = lastA; + centerA = (minA + maxA) / 2; + + if (lastB < minB) + minB = lastB; + if (lastB > maxB) + maxB = lastB; + centerB = (minB + maxB) / 2; + + _delay(2); + } + + //establish initial reading for rollover handling + electrical_rev = 0; + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h new file mode 100644 index 00000000..f05b7b2b --- /dev/null +++ b/src/sensors/LinearHall.h @@ -0,0 +1,45 @@ +#ifndef LINEAR_HALL_SENSOR_LIB_H +#define LINEAR_HALL_SENSOR_LIB_H + +#include + +class FOCMotor; +void ReadLinearHalls(int hallA, int hallB, int *a, int *b); + +/** + * This sensor class is for two linear hall effect sensors such as 49E, which are + * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, + * the other is half way between rotor magnets). + * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). + * + * For more information, see this forum thread and PDF + * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 + * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + */ +class LinearHall: public Sensor{ + public: + LinearHall(int hallA, int hallB, int pp); + + void init(int centerA, int centerB); // Initialize without moving motor + void init(FOCMotor *motor); // Move motor to find center values + + // Get current shaft angle from the sensor hardware, and + // return it as a float in radians, in the range 0 to 2PI. + // - This method is pure virtual and must be implemented in subclasses. + // Calling this method directly does not update the base-class internal fields. + // Use update() when calling from outside code. + float getSensorAngle() override; + + int centerA; + int centerB; + int lastA, lastB; + + private: + int pinA; + int pinB; + int pp; + int electrical_rev; + float prev_reading; +}; + +#endif From aeb923824dd2366ca341f2cabcc1cabf063d814c Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:40:04 -0600 Subject: [PATCH 115/220] Minor formatting/comment edits --- src/sensors/LinearHall.cpp | 5 +++-- src/sensors/LinearHall.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp index 36303aaa..96cfae3e 100644 --- a/src/sensors/LinearHall.cpp +++ b/src/sensors/LinearHall.cpp @@ -3,8 +3,8 @@ // This function can be overridden with custom ADC code on platforms with poor analogRead performance. __attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) { - *a = analogRead(hallA); - *b = analogRead(hallB); + *a = analogRead(hallA); + *b = analogRead(hallB); } LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ @@ -71,6 +71,7 @@ void LinearHall::init(FOCMotor *motor) { return; } + // See comment in other version of init for why these are commented out //pinMode(pinA, INPUT); //pinMode(pinB, INPUT); diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h index f05b7b2b..d17035e4 100644 --- a/src/sensors/LinearHall.h +++ b/src/sensors/LinearHall.h @@ -3,7 +3,7 @@ #include -class FOCMotor; +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. void ReadLinearHalls(int hallA, int hallB, int *a, int *b); /** @@ -21,7 +21,7 @@ class LinearHall: public Sensor{ LinearHall(int hallA, int hallB, int pp); void init(int centerA, int centerB); // Initialize without moving motor - void init(FOCMotor *motor); // Move motor to find center values + void init(class FOCMotor *motor); // Move motor to find center values // Get current shaft angle from the sensor hardware, and // return it as a float in radians, in the range 0 to 2PI. From 3bcb9af9fc4564d3cb998d3a83ee8a381ee69e23 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Jan 2024 23:32:25 +0100 Subject: [PATCH 116/220] expose result of PP check --- src/BLDCMotor.cpp | 3 ++- src/StepperMotor.cpp | 3 ++- src/common/base_classes/FOCMotor.h | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8fd54ba6..e0f5415d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -259,7 +259,8 @@ int BLDCMotor::alignSensor() { sensor_direction = Direction::CW; } // check pole pair number - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 6cbe417a..697ed277 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -175,7 +175,8 @@ int StepperMotor::alignSensor() { } // check pole pair number float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 318be99a..b5b0a557 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -206,6 +206,7 @@ class FOCMotor float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration + bool pp_check_result = false; //!< the result of the PP check, if run during loopFOC /** * Function providing BLDCMotor class with the From b2da2b96822f466221b04715964add00d5457efb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 21 Jan 2024 00:45:00 +0100 Subject: [PATCH 117/220] respect pin nrs on boards which remap them --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index eee5797b..f187fbef 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -2,6 +2,10 @@ /** * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. */ + +#include "./rp2040_mcu.h" + + #if defined(TARGET_RP2040) @@ -13,9 +17,9 @@ #define SIMPLEFOC_DEBUG_RP2040 #include "../../hardware_api.h" -#include "./rp2040_mcu.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#include #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -30,7 +34,8 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { +void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); @@ -45,7 +50,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); - SimpleFOCDebug::print(pin); + SimpleFOCDebug::print((int)pin); SimpleFOCDebug::print(" slice "); SimpleFOCDebug::print((int)slice); SimpleFOCDebug::print(" channel "); From 50fc6de70c17bcc4fba00a0347df0596bebf2ffc Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Sun, 21 Jan 2024 11:21:28 +0100 Subject: [PATCH 118/220] initFOC: remove unnecessary delay --- src/BLDCMotor.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index e0f5415d..9dbc0a48 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -151,7 +151,6 @@ int BLDCMotor::initFOC() { // alignment necessary for encoders! // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update @@ -165,7 +164,6 @@ int BLDCMotor::initFOC() { // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases // and checks the direction of measuremnt. - _delay(500); if(exit_flag){ if(current_sense){ if (!current_sense->initialized) { From 813917a3ed68485f0ee5646869403b306363416a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 26 Jan 2024 18:08:19 +0100 Subject: [PATCH 119/220] Force the use of the ADC_INJECTED_RANK_x instead of the index --- .../hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp | 6 ++++-- .../hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp | 4 +++- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 5 ++++- .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 4 +++- 4 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 3a97dc54..69fea017 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -88,8 +88,10 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 3a7b131c..a3ee5407 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -78,7 +78,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 4e1aa9ac..1a954105 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -11,6 +11,7 @@ #include "stm32g4_utils.h" #include "Arduino.h" +// #define SIMPLEFOC_STM32_ADC_INTERRUPT #define _ADC_VOLTAGE_G4 3.3f #define _ADC_RESOLUTION_G4 4096.0f @@ -84,7 +85,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index d1ca2814..9dea0933 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -84,7 +84,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } From 36e3da807c0d1edb17b80474524af37267a3cb35 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 26 Jan 2024 21:55:52 +0100 Subject: [PATCH 120/220] fix floating point constant incorrectly converted to int --- src/common/foc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fa937d15..c938d363 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { From a49da451503356a2bb3443303b7e2751dcc979c1 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 28 Jan 2024 14:23:56 +0100 Subject: [PATCH 121/220] initial version of comprehensive timer synchornisation for truly center aligned PWM for stm32 (when using more than one timer) --- .../hardware_specific/stm32/stm32_mcu.cpp | 118 +++++++++++++++--- 1 file changed, 99 insertions(+), 19 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 65dad9f4..db7429ac 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,6 +4,7 @@ #if defined(_STM32_DEF_) +#define SIMPLEFOC_STM32_DEBUG #pragma message("") #pragma message("SimpleFOC: compiling for STM32") #pragma message("") @@ -204,25 +205,43 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } } -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // TODO - sart each timer only once - // sart timers - 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 - } + +// function finds the appropriate timer source trigger for the master timer index provided +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM8 +int _getTriggerSourceRegister(HardwareTimer* timer) { + #if defined(TIM1) && TIM_TS_ITR0 + if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + #endif + #if defined(TIM2) && TIM_TS_ITR1 + if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + #endif + #if defined(TIM3) && TIM_TS_ITR2 + if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + #endif + #if defined(TIM4) && TIM_TS_ITR3 + if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + #endif + #if defined(TIM5) && TIM_TS_ITR4 + if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + #endif + #if defined(TIM6) && TIM_TS_ITR5 + if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + #endif + #if defined(TIM7) && TIM_TS_ITR6 + if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; + #endif + #if defined(TIM8) && TIM_TS_ITR7 + if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; + #endif + return -1; } void _alignTimersNew() { int numTimers = 0; HardwareTimer *timers[numTimerPinsUsed]; - // reset timer counters + // find the timers used for (int i=0; iperipheral); HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); @@ -237,6 +256,55 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + // see if there is more then 1 timers used for the pwm + // if yes, try to align timers + if(numTimers > 1){ + // find the master timer + uint8_t masterTimerIndex = 0; + int triggerEvent = -1; + for (int i=0; igetHandle()->Instance)) { + // check if timer already configured in TRGO update mode (used for ADC triggering) + // in that case we should not change its TRGO configuration + if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; + // check if it has the supported internal trigger + triggerEvent = _getTriggerSourceRegister(timers[i]); + if(triggerEvent == -1) continue; // not supported keep searching + masterTimerIndex = i; // found the master timer + break; + } + } + + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: aligning!"); + #endif + + // if no master timer found do not perform alignment + if (triggerEvent == -1) { + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: ERR: No master timer found, cannot align timers!"); + #endif + }else{ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[masterTimerIndex]->getHandle()->Instance))); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode + LL_TIM_SetSlaveMode(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + // configure other timers to get the input trigger from the master timer + for (int i=0; igetHandle()->Instance, triggerEvent); + LL_TIM_SetSlaveMode(timers[i]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + } + } + } + // enable timer clock for (int i=0; ipause(); @@ -254,6 +322,20 @@ void _alignTimersNew() { +// align the timers to end the init +void _startTimers(HardwareTimer **timers_to_start, int timer_num) +{ + // // TODO - start each timer only once + // // start timers + // 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 + // } + _alignTimersNew(); +} // configure hardware 6pwm for a complementary pair of channels @@ -540,7 +622,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // allign the timers + // align the timers _alignTimersNew(); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); @@ -598,6 +680,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +TIM_MasterConfigTypeDef sMasterConfig; +TIM_SlaveConfigTypeDef sSlaveConfig; // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -620,7 +704,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -945,8 +1029,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif - - - - #endif From ce7a0327aba428fd570f016c2f311aa025f42ddb Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 28 Jan 2024 14:44:43 +0100 Subject: [PATCH 122/220] removed alignement message --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index db7429ac..a9d2b9ce 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -256,6 +256,10 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers!\nTimer no. ", numTimers); + #endif + // see if there is more then 1 timers used for the pwm // if yes, try to align timers if(numTimers > 1){ @@ -276,9 +280,6 @@ void _alignTimersNew() { } } - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: aligning!"); - #endif // if no master timer found do not perform alignment if (triggerEvent == -1) { From adbc55a2a6b64f4dd7dce01c6012ee2f80c09b8e Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 29 Jan 2024 20:04:47 +0100 Subject: [PATCH 123/220] bugfix forgotten define check for TRGO events --- .../hardware_specific/stm32/stm32_mcu.cpp | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index a9d2b9ce..f9a2e847 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -10,8 +10,6 @@ #pragma message("") -//#define SIMPLEFOC_STM32_DEBUG - #ifdef SIMPLEFOC_STM32_DEBUG void printTimerCombination(int numPins, PinMap* timers[], int score); int getTimerNumber(int timerIndex); @@ -210,28 +208,28 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM8 int _getTriggerSourceRegister(HardwareTimer* timer) { - #if defined(TIM1) && TIM_TS_ITR0 + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; #endif - #if defined(TIM2) && TIM_TS_ITR1 + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; #endif - #if defined(TIM3) && TIM_TS_ITR2 + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; #endif - #if defined(TIM4) && TIM_TS_ITR3 + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; #endif - #if defined(TIM5) && TIM_TS_ITR4 + #if defined(TIM5) && defined(LL_TIM_TS_ITR4) if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif - #if defined(TIM6) && TIM_TS_ITR5 + #if defined(TIM6) && defined(LL_TIM_TS_ITR5) if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif - #if defined(TIM7) && TIM_TS_ITR6 + #if defined(TIM7) && defined(LL_TIM_TS_ITR6) if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; #endif - #if defined(TIM8) && TIM_TS_ITR7 + #if defined(TIM8) && defined(LL_TIM_TS_ITR7) if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; #endif return -1; @@ -256,9 +254,9 @@ void _alignTimersNew() { timers[numTimers++] = timer; } - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers!\nTimer no. ", numTimers); - #endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + #endif // see if there is more then 1 timers used for the pwm // if yes, try to align timers From 2d09173d03e4b498ad42afe540ff97c52932d5c5 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 08:32:34 +0100 Subject: [PATCH 124/220] remove timers 6 and 7 --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index f9a2e847..035e0dbd 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -206,7 +206,7 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) // function finds the appropriate timer source trigger for the master timer index provided // returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM8 +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 int _getTriggerSourceRegister(HardwareTimer* timer) { #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; @@ -223,14 +223,8 @@ int _getTriggerSourceRegister(HardwareTimer* timer) { #if defined(TIM5) && defined(LL_TIM_TS_ITR4) if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif - #if defined(TIM6) && defined(LL_TIM_TS_ITR5) - if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; - #endif - #if defined(TIM7) && defined(LL_TIM_TS_ITR6) - if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; - #endif - #if defined(TIM8) && defined(LL_TIM_TS_ITR7) - if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; + #if defined(TIM8) && defined(LL_TIM_TS_ITR5) + if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif return -1; } From 6aed7803de0bf0ef4a5c3ceff061bca224a629d6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 20:12:12 +0100 Subject: [PATCH 125/220] undate on the alignement logic --- .../hardware_specific/stm32/stm32_mcu.cpp | 123 +++++++++++++++--- 1 file changed, 102 insertions(+), 21 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 035e0dbd..e4305f41 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -204,30 +204,105 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } -// function finds the appropriate timer source trigger for the master timer index provided +#if defined(STM32G4xx) +// function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getTriggerSourceRegister(HardwareTimer* timer) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; #endif #if defined(TIM2) && defined(LL_TIM_TS_ITR1) - if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; #endif #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; #endif #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; #endif #if defined(TIM5) && defined(LL_TIM_TS_ITR4) - if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif #if defined(TIM8) && defined(LL_TIM_TS_ITR5) - if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif return -1; } +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) + +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + if(TIM_slave == TIM2 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + if(TIM_slave == TIM1 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + if(TIM_slave== TIM1 || TIM_slave == TIM2 || TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + if(TIM_slave == TIM1 || TIM_slave == TIM2 || TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + else if(TIM_slave ==TIM4 || TIM_slave ==TIM5) return LL_TIM_TS_ITR3; + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { + return -1; +} +#endif + void _alignTimersNew() { int numTimers = 0; @@ -256,7 +331,7 @@ void _alignTimersNew() { // if yes, try to align timers if(numTimers > 1){ // find the master timer - uint8_t masterTimerIndex = 0; + int16_t master_index = -1; int triggerEvent = -1; for (int i=0; igetHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; - // check if it has the supported internal trigger - triggerEvent = _getTriggerSourceRegister(timers[i]); - if(triggerEvent == -1) continue; // not supported keep searching - masterTimerIndex = i; // found the master timer + // check if the timer has the supported internal trigger for other timers + for (int slave_i=0; slave_igetHandle()->Instance))); + SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[master_index]->getHandle()->Instance))); #endif // make the master timer generate ITRGx event // if it was already configured in slave mode - LL_TIM_SetSlaveMode(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); // Configure the master timer to send a trigger signal on enable - LL_TIM_SetTriggerOutput(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); // configure other timers to get the input trigger from the master timer - for (int i=0; igetHandle()->Instance, triggerEvent); - LL_TIM_SetSlaveMode(timers[i]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getTriggerSourceRegister(timers[master_index], timers[slave_index])); + LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); } } } From 3ad11eebafb13e3603b6e32dfbea8903cfd775c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 20:17:49 +0100 Subject: [PATCH 126/220] typo function name --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index e4305f41..bfb25789 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -235,7 +235,7 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows TIM_TypeDef *TIM_master = master->getHandle()->Instance; TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; @@ -298,7 +298,7 @@ int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { } #else // Alignment not supported for this architecture -int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { return -1; } #endif @@ -343,7 +343,7 @@ void _alignTimersNew() { for (int slave_i=0; slave_igetHandle()->Instance, _getTriggerSourceRegister(timers[master_index], timers[slave_index])); + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); } } From b34bd626526acc0c66b8698ca46e15d17dead021 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 3 Feb 2024 16:41:25 +0100 Subject: [PATCH 127/220] initial support for teensy4 low-side current sensing (only for 6pwm at the motment) - in progress --- .../hardware_specific/teensy/teensy4_mcu.cpp | 191 ++++++++++++++++++ .../hardware_specific/teensy/teensy4_mcu.h | 77 +++++++ .../{ => teensy}/teensy_mcu.cpp | 2 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 14 +- 4 files changed, 273 insertions(+), 11 deletions(-) create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.h rename src/current_sense/hardware_specific/{ => teensy}/teensy_mcu.cpp (94%) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..8f2b441c --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,191 @@ +#include "teensy4_mcu.h" +#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 +int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +volatile uint32_t val0, val1; + +void read_currents(uint32_t *a, uint32_t*b){ + *a = val0; + *b = val1; +} + +// interrupt service routine for the ADC_ETC0 +// reading the ADC values and clearing the interrupt +void adcetc0_isr() { + digitalWrite(30,HIGH); + ADC_ETC_DONE0_1_IRQ |= 1; // clear + val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc1_init() { + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + + //Calibration of ADC1 + ADC1_GC |= ADC_GC_CAL; // begin cal ADC1 + while (ADC1_GC & ADC_GC_CAL) ; + + ADC1_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC1_HC1 = 16; +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc2_init(){ + + // configuring ADC2 + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + //Calibration of ADC2 + ADC2_GC |= ADC_GC_CAL; // begin cal ADC2 + while (ADC2_GC & ADC_GC_CAL) ; + + ADC2_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC2_HC1 = 16; +} + +void adc_etc_init(int pin1, int pin2) { + ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST + ADC_ETC_CTRL = 0x40000001; // start with trigger 0 + ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + + // ADC1 7 8, chain channel, HWTS, IE, B2B + // pg 3516, section 66.5.1.8 + ADC_ETC_TRIG0_CHAIN_1_0 = + ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS1(1) | + ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8 + ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7 + + attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); + // use the second interrupt if necessary (for more than 2 channels) + // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); +} + + +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + + GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; + float adc_voltage_conv = params->adc_voltage_conv; + if (pinA == params->pins[0]) { + return val0 * adc_voltage_conv; + } else if (pinA == params->pins[1]) { + return val1 * adc_voltage_conv; + } + return 0.0; +} + +// Configure low side for generic mcu +// cannot do much but +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + pinMode(30,OUTPUT); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + adc1_init(); + adc_etc_init(pinA, pinB); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + return params; +} + +// sync driver and the adc +void _driverSyncLowSide(void* driver_params, void* cs_params){ + Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; + int submodule = par->submodules[0]; + + // do xbar connect here + + int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + if(xbar_trig_pwm<0) return; + + xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + + // setup the ADC_ETC trigger + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // setup this val4 for interrupt on val5 match for ADC sync + // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.h b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h new file mode 100644 index 00000000..2cf77dfb --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,77 @@ + +#ifndef TEENSY4_CURRENTSENSE_MCU_DEF +#define TEENSY4_CURRENTSENSE_MCU_DEF + +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4026.0f + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct Teensy4CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; +} Teensy4CurrentSenseParams; + + + +const uint8_t pin_to_channel[] = { // pg 482 + 7, // 0/A0 AD_B1_02 + 8, // 1/A1 AD_B1_03 + 12, // 2/A2 AD_B1_07 + 11, // 3/A3 AD_B1_06 + 6, // 4/A4 AD_B1_01 + 5, // 5/A5 AD_B1_00 + 15, // 6/A6 AD_B1_10 + 0, // 7/A7 AD_B1_11 + 13, // 8/A8 AD_B1_08 + 14, // 9/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 + 7, // 14/A0 AD_B1_02 + 8, // 15/A1 AD_B1_03 + 12, // 16/A2 AD_B1_07 + 11, // 17/A3 AD_B1_06 + 6, // 18/A4 AD_B1_01 + 5, // 19/A5 AD_B1_00 + 15, // 20/A6 AD_B1_10 + 0, // 21/A7 AD_B1_11 + 13, // 22/A8 AD_B1_08 + 14, // 23/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 +#ifdef ARDUINO_TEENSY41 + 255, // 28 + 255, // 29 + 255, // 30 + 255, // 31 + 255, // 32 + 255, // 33 + 255, // 34 + 255, // 35 + 255, // 36 + 255, // 37 + 128+1, // 38/A14 AD_B1_12 - only on ADC2, 1 + 128+2, // 39/A15 AD_B1_13 - only on ADC2, 2 + 9, // 40/A16 AD_B1_04 + 10, // 41/A17 AD_B1_05 +#endif +}; + + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp similarity index 94% rename from src/current_sense/hardware_specific/teensy_mcu.cpp rename to src/current_sense/hardware_specific/teensy/teensy_mcu.cpp index 7ab370a4..7669edc8 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 322d5a34..d3738aed 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -177,18 +177,15 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match. flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0 ; + flexpwm->SM[submodule].VAL0 = 0; flexpwm->SM[submodule].VAL1 = half_cycle ; flexpwm->SM[submodule].VAL2 = -mid_pwm ; flexpwm->SM[submodule].VAL3 = +mid_pwm ; - // flexpwm->SM[submodule].VAL4 = -mid_pwm ; - // flexpwm->SM[submodule].VAL5 = +mid_pwm ; - + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running } @@ -207,14 +204,11 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; flexpwm->SM[submodule].VAL2 = count_pwm; // A on flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off - // flexpwm->SM[submodule].VAL4 = - count_pwm ; // B off (assuming B inverted) - // flexpwm->SM[submodule].VAL5 = + count_pwm ; // B on flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1< Date: Tue, 6 Feb 2024 10:27:35 +0100 Subject: [PATCH 128/220] Use interrupt if no repetition counter --- .../stm32/stm32f1/stm32f1_hal.cpp | 9 ----- .../stm32/stm32f1/stm32f1_mcu.cpp | 36 +++++++++++++------ 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index ec75ef4f..3dd9b8de 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -124,12 +124,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - #endif - cs_params->adc_handle = &hadc; return 0; @@ -153,14 +147,11 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } - } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 69fea017..9d83cc4f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -19,6 +19,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ if(AdcHandle->Instance == ADC1) return 0; #ifdef ADC2 // if ADC2 exists @@ -70,12 +76,24 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); + if( !use_adc_interrupt && !IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance)){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } + // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + if(use_adc_interrupt){ + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -86,19 +104,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -115,6 +132,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file From 24be38019e17b84a6673d07644f89c24a9f43daa Mon Sep 17 00:00:00 2001 From: Candas1 Date: Wed, 7 Feb 2024 16:13:12 +0100 Subject: [PATCH 129/220] Update stm32_mcu.cpp Comment the refresh of the timer in the new alignment code, it seems to mess up with the low side current sampling --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index bfb25789..33b5b664 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -382,7 +382,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + //timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif From 83826036937ee429cff17fb762355b9cc8758d18 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Wed, 7 Feb 2024 21:00:36 +0100 Subject: [PATCH 130/220] Same for f4,g4 and l4 --- .../stm32/stm32f1/stm32f1_mcu.cpp | 16 ++--- .../stm32/stm32f4/stm32f4_hal.cpp | 8 --- .../stm32/stm32f4/stm32f4_mcu.cpp | 36 +++++++--- .../stm32/stm32g4/stm32g4_hal.cpp | 39 ----------- .../stm32/stm32g4/stm32g4_mcu.cpp | 68 ++++++++++++++++--- .../stm32/stm32l4/stm32l4_hal.cpp | 39 ----------- .../stm32/stm32l4/stm32l4_mcu.cpp | 60 +++++++++++++--- .../hardware_specific/stm32/stm32_mcu.cpp | 2 +- 8 files changed, 144 insertions(+), 124 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 9d83cc4f..5f090c20 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -69,20 +69,20 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); - - if( !use_adc_interrupt && !IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance)){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); - #endif - } // start the adc if(use_adc_interrupt){ diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index dcf32137..bd0df4b6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,12 +135,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - // enable interrupt - HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC_IRQn); - #endif - cs_params->adc_handle = &hadc; return 0; } @@ -162,13 +156,11 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index a3ee5407..6388e8e0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -22,6 +22,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -55,16 +61,28 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + if (use_adc_interrupt){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -75,19 +93,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -104,6 +121,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index bf89463b..e9210d54 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -180,43 +180,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif -#endif - cs_params->adc_handle = &hadc; return 0; } @@ -238,7 +201,6 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -265,6 +227,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 1a954105..fb32d36b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -24,6 +24,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -58,6 +63,14 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event @@ -66,12 +79,47 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); - // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + // start the adc + if (use_adc_interrupt){ + // enable interrupt + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -82,19 +130,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -111,6 +158,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index 688ba796..d728e076 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -179,43 +179,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif -#endif - cs_params->adc_handle = &hadc; return 0; } @@ -237,7 +200,6 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -264,6 +226,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 9dea0933..1fb0ab6b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -23,6 +23,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -57,6 +62,14 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event @@ -66,11 +79,44 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else + }else{ HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -81,19 +127,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -110,6 +155,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index bfb25789..33b5b664 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -382,7 +382,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + //timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif From d7d44887737f6dd949c2fc06bb7a153d9b026b36 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Fri, 9 Feb 2024 13:00:50 +0100 Subject: [PATCH 131/220] optimize --- src/sensors/HallSensor.cpp | 29 +++++++++++++++++++---------- src/sensors/HallSensor.h | 1 + 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index d9e5ec83..64c5e48c 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -42,22 +42,21 @@ void HallSensor::handleC() { * Updates the state and sector following an interrupt */ void HallSensor::updateState() { - long new_pulse_timestamp = _micros(); - int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed - if (new_hall_state == hall_state) { - return; - } + if (new_hall_state == hall_state) return; + + long new_pulse_timestamp = _micros(); hall_state = new_hall_state; int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; - if (new_electric_sector - electric_sector > 3) { + int8_t electric_sector_dif = new_electric_sector - electric_sector; + if (electric_sector_dif > 3) { //underflow direction = Direction::CCW; electric_rotations += direction; - } else if (new_electric_sector - electric_sector < (-3)) { + } else if (electric_sector_dif < (-3)) { //overflow direction = Direction::CW; electric_rotations += direction; @@ -96,11 +95,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void HallSensor::update() { // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed - noInterrupts(); + if (use_interrupt){ + noInterrupts(); + }else{ + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + } + angle_prev_ts = pulse_timestamp; long last_electric_rotations = electric_rotations; int8_t last_electric_sector = electric_sector; - interrupts(); + if (use_interrupt) interrupts(); angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -150,7 +157,7 @@ void HallSensor::init(){ } // init hall_state - A_active= digitalRead(pinA); + A_active = digitalRead(pinA); B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); @@ -169,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); + + use_interrupt = true; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 78e1b416..94053e0f 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -47,6 +47,7 @@ class HallSensor: public Sensor{ int pinA; //!< HallSensor hardware pin A int pinB; //!< HallSensor hardware pin B int pinC; //!< HallSensor hardware pin C + int use_interrupt; //!< True if interrupts have been attached // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups From 81db3f5a428efe8b85d712ab54b7beef62ac05f8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 11 Feb 2024 19:47:10 +0100 Subject: [PATCH 132/220] added support for more than 2/3 adc pins --- .../hardware_specific/teensy/teensy4_mcu.cpp | 89 ++++++++++++++----- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 8f2b441c..eca862a1 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +#include "../../../common/foc_utils.h" // if defined // - Teensy 4.0 @@ -12,7 +13,7 @@ // // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ if(submodule <0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; @@ -26,32 +27,45 @@ int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } -volatile uint32_t val0, val1; +volatile uint32_t val0, val1, val2; -void read_currents(uint32_t *a, uint32_t*b){ +void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; + *c = val2; } // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { digitalWrite(30,HIGH); - ADC_ETC_DONE0_1_IRQ |= 1; // clear + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; asm("dsb"); digitalWrite(30,LOW); } + +void adcetc1_isr() { + digitalWrite(30,HIGH); + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit + val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; + // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + // function initializing the ADC2 // and the ADC_ETC trigger for the low side current sensing -void adc1_init() { +void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { //Tried many configurations, but this seems to be best: ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -63,7 +77,9 @@ void adc1_init() { ADC1_HC0 = 16; // ADC_ETC channel // use the second interrupt if necessary (for more than 2 channels) - // ADC1_HC1 = 16; + if(_isset(pin3)) { + ADC1_HC1 = 16; + } } // function initializing the ADC2 @@ -75,7 +91,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -89,10 +105,13 @@ void adc2_init(){ // ADC2_HC1 = 16; } -void adc_etc_init(int pin1, int pin2) { +// function initializing the ADC_ETC trigger for the low side current sensing +// it uses only the ADC1 +// if the pin3 is not set it uses only 2 channels +void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST ADC_ETC_CTRL = 0x40000001; // start with trigger 0 - ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels // ADC1 7 8, chain channel, HWTS, IE, B2B // pg 3516, section 66.5.1.8 @@ -109,11 +128,18 @@ void adc_etc_init(int pin1, int pin2) { attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); // use the second interrupt if necessary (for more than 2 channels) - // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); - // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + if(_isset(pin3)) { + ADC_ETC_TRIG0_CHAIN_3_2 = + ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]); + + attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + } } - void xbar_connect(unsigned int input, unsigned int output) { if (input >= 88) return; @@ -135,12 +161,15 @@ void xbar_init() { // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + if(!_isset(pinA)) return 0.0; // if the pin is not set return 0 GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; float adc_voltage_conv = params->adc_voltage_conv; if (pinA == params->pins[0]) { return val0 * adc_voltage_conv; } else if (pinA == params->pins[1]) { return val1 * adc_voltage_conv; + }else if (pinA == params->pins[2]) { + return val2 * adc_voltage_conv; } return 0.0; } @@ -149,6 +178,7 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); + // _UNUSED(pinC); pinMode(30,OUTPUT); @@ -156,11 +186,25 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - adc1_init(); - adc_etc_init(pinA, pinB); + // check if either of the pins are not set + // and dont use it if it isn't + int pin_count = 0; + int pins[3] = {NOT_SET, NOT_SET, NOT_SET}; + if(_isset(pinA)) pins[pin_count++] = pinA; + if(_isset(pinB)) pins[pin_count++] = pinB; + if(_isset(pinC)) pins[pin_count++] = pinC; + + + adc1_init(pins[0], pins[1], pins[2]); + SIMPLEFOC_DEBUG("pins: ",pins[0]); + SIMPLEFOC_DEBUG("pins: ",pins[1]); + SIMPLEFOC_DEBUG("pins: ",pins[2]); + adc_etc_init(pins[0], pins[1], pins[2]); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, + .pins = {pins[0], pins[1], pins[2] }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; return params; @@ -172,17 +216,18 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; - // do xbar connect here - - int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + // find the xbar trigger for the flexpwm + int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; + // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc - // setup the ADC_ETC trigger + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); - // setup this val4 for interrupt on val5 match for ADC sync - // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + // setup this val4 for interrupt on match for ADC sync + // this code assumes that the val4 is not used for anything else! + // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center } From 41de6b673ebb6528e90155f793019a72f47d8435 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:38:50 -0600 Subject: [PATCH 133/220] Moving LinearHall back to drivers repository As requested by runger --- src/sensors/LinearHall.cpp | 110 ------------------------------------- src/sensors/LinearHall.h | 45 --------------- 2 files changed, 155 deletions(-) delete mode 100644 src/sensors/LinearHall.cpp delete mode 100644 src/sensors/LinearHall.h diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp deleted file mode 100644 index 96cfae3e..00000000 --- a/src/sensors/LinearHall.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include "LinearHall.h" - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) -{ - *a = analogRead(hallA); - *b = analogRead(hallB); -} - -LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ - centerA = 512; - centerB = 512; - pinA = _hallA; - pinB = _hallB; - pp = _pp; -} - -float LinearHall::getSensorAngle() { - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - //offset readings using center values, then compute angle - float reading = _atan2(lastA - centerA, lastB - centerB); - - //handle rollover logic between each electrical revolution of the motor - if (reading > prev_reading) { - if (reading - prev_reading >= PI) { - if (electrical_rev - 1 < 0) { - electrical_rev = pp - 1; - } else { - electrical_rev = electrical_rev - 1; - } - } - } else if (reading < prev_reading) { - if (prev_reading - reading >= PI) { - if (electrical_rev + 1 >= pp) { - electrical_rev = 0; - } else { - electrical_rev = electrical_rev + 1; - } - } - } - - //convert result from electrical angle and electrical revolution count to shaft angle in radians - float result = (reading + PI) / _2PI; - result = _2PI * (result + electrical_rev) / pp; - - //update previous reading for rollover handling - prev_reading = reading; - return result; -} - -void LinearHall::init(int _centerA, int _centerB) { - // Skip configuring the pins here because they normally default to input anyway, and - // this makes it possible to use ADC channel numbers instead of pin numbers when using - // custom implementation of ReadLinearHalls, to avoid having to remap them every update. - // If pins do need to be configured, it can be done by user code before calling init. - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - centerA = _centerA; - centerB = _centerB; - - //establish initial reading for rollover handling - electrical_rev = 0; - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} - -void LinearHall::init(FOCMotor *motor) { - if (!motor->enabled) { - SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); - return; - } - - // See comment in other version of init for why these are commented out - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - int minA, maxA, minB, maxB; - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - minA = maxA = centerA = lastA; - minB = maxB = centerB = lastB; - - // move one mechanical revolution forward - for (int i = 0; i <= 2000; i++) - { - float angle = _3PI_2 + _2PI * i * pp / 2000.0f; - motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - - if (lastA < minA) - minA = lastA; - if (lastA > maxA) - maxA = lastA; - centerA = (minA + maxA) / 2; - - if (lastB < minB) - minB = lastB; - if (lastB > maxB) - maxB = lastB; - centerB = (minB + maxB) / 2; - - _delay(2); - } - - //establish initial reading for rollover handling - electrical_rev = 0; - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h deleted file mode 100644 index d17035e4..00000000 --- a/src/sensors/LinearHall.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef LINEAR_HALL_SENSOR_LIB_H -#define LINEAR_HALL_SENSOR_LIB_H - -#include - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -void ReadLinearHalls(int hallA, int hallB, int *a, int *b); - -/** - * This sensor class is for two linear hall effect sensors such as 49E, which are - * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, - * the other is half way between rotor magnets). - * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). - * - * For more information, see this forum thread and PDF - * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 - * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 - */ -class LinearHall: public Sensor{ - public: - LinearHall(int hallA, int hallB, int pp); - - void init(int centerA, int centerB); // Initialize without moving motor - void init(class FOCMotor *motor); // Move motor to find center values - - // Get current shaft angle from the sensor hardware, and - // return it as a float in radians, in the range 0 to 2PI. - // - This method is pure virtual and must be implemented in subclasses. - // Calling this method directly does not update the base-class internal fields. - // Use update() when calling from outside code. - float getSensorAngle() override; - - int centerA; - int centerB; - int lastA, lastB; - - private: - int pinA; - int pinB; - int pp; - int electrical_rev; - float prev_reading; -}; - -#endif From 3a840213de36eddf5219e691b1f8482d5147c490 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:53:46 -0600 Subject: [PATCH 134/220] Moving LinearHall back to drivers repository --- .../angle_control/angle_control.ino | 117 ------------------ .../voltage_control/voltage_control.ino | 96 -------------- .../velocity_control/velocity_control.ino | 109 ---------------- 3 files changed, 322 deletions(-) delete mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino delete mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino delete mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino deleted file mode 100644 index 8179008d..00000000 --- a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino +++ /dev/null @@ -1,117 +0,0 @@ -/** - * - * Position/angle motion control example - * Steps: - * 1) Configure the motor and hall sensor - * 2) Run the code - * 3) Set the target angle (in radians) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// angle set point variable -float target_angle = 0; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - - // aligning voltage [V] - motor.voltage_sensor_align = 3; - // index search velocity [rad/s] - motor.velocity_index_search = 3; - - // set motion control loop to be used - motor.controller = MotionControlType::angle; - - // contoller configuration - // default parameters in defaults.h - - // velocity PI controller parameters - motor.PID_velocity.P = 0.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // default voltage_power_supply - motor.voltage_limit = 6; - // jerk control using voltage voltage ramp - // default value is 300 volts per sec ~ 0.3V per millisecond - motor.PID_velocity.output_ramp = 1000; - - // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01f; - - // angle P controller - motor.P_angle.P = 20; - // maximal velocity of the position control - motor.velocity_limit = 4; - - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target angle"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target angle using serial terminal:")); - _delay(1000); -} - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_angle); - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor.monitor(); - - // user communication - command.run(); -} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino deleted file mode 100644 index e646849a..00000000 --- a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino +++ /dev/null @@ -1,96 +0,0 @@ -/** - * - * Torque control example using voltage control loop. - * - * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers - * you a way to control motor torque by setting the voltage to the motor instead of the current. - * - * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. - */ -#include - - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - - -// voltage set point variable -float target_voltage = 2; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link driver - motor.linkDriver(&driver); - - // aligning voltage - motor.voltage_sensor_align = 3; - - // choose FOC modulation (optional) - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - - // set motion control loop to be used - motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); - _delay(1000); -} - - -void loop() { - - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_voltage); - - // user communication - command.run(); -} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino deleted file mode 100644 index 870a0899..00000000 --- a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino +++ /dev/null @@ -1,109 +0,0 @@ -/** - * - * Velocity motion control example - * Steps: - * 1) Configure the motor and sensor - * 2) Run the code - * 3) Set the target velocity (in radians per second) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// velocity set point variable -float target_velocity = 0; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - // aligning voltage [V] - motor.voltage_sensor_align = 3; - - // set motion control loop to be used - motor.controller = MotionControlType::velocity; - - // contoller configuration - // default parameters in defaults.h - - // velocity PI controller parameters - motor.PID_velocity.P = 0.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // default voltage_power_supply - motor.voltage_limit = 6; - // jerk control using voltage voltage ramp - // default value is 300 volts per sec ~ 0.3V per millisecond - motor.PID_velocity.output_ramp = 1000; - - // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01f; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target velocity using serial terminal:")); - _delay(1000); -} - - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_velocity); - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor.monitor(); - - // user communication - command.run(); -} \ No newline at end of file From 1ecfb22ef35422e6d0d1ae8eb59821bd96b0c7eb Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 18 Feb 2024 20:28:55 +0100 Subject: [PATCH 135/220] NbrOfConversion = 1 --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 2 +- .../hardware_specific/stm32/stm32g4/stm32g4_hal.cpp | 2 +- .../hardware_specific/stm32/stm32l4/stm32l4_hal.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..245f27f7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -67,7 +67,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.DiscontinuousConvMode = DISABLE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 0; + hadc.Init.NbrOfConversion = 1; HAL_ADC_Init(&hadc); /**Configure for the selected ADC regular channel to be converted. */ diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 91322112..c62ba07a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -100,7 +100,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..76336154 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -99,7 +99,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; From 0588d3845188237acf070fc0a5cab50fa6f286d4 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 26 Feb 2024 17:41:35 +0100 Subject: [PATCH 136/220] added teensy 4 6pwm tiemer sync and better support for low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 94 +++++----- .../hardware_specific/teensy/teensy4_mcu.cpp | 164 ++++++++++++++++-- .../hardware_specific/teensy/teensy4_mcu.h | 11 ++ 3 files changed, 207 insertions(+), 62 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index eca862a1..b76624bc 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +// #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" // if defined @@ -7,28 +8,16 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} +// #define TEENSY4_ADC_INTERRUPT_DEBUG + +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); + void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; @@ -38,24 +27,32 @@ void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit - val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + // val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095); + val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); + // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; - asm("dsb"); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); +#endif } void adcetc1_isr() { - digitalWrite(30,HIGH); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; - // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; - asm("dsb"); - digitalWrite(30,LOW); +// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif } // function initializing the ADC2 @@ -65,7 +62,7 @@ void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) (1 is faster and maybe with some filtering could provide better results but 2 for now) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -91,7 +88,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -140,22 +137,6 @@ void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { } } -void xbar_connect(unsigned int input, unsigned int output) -{ - if (input >= 88) return; - if (output >= 132) return; - volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); - uint16_t val = *xbar; - if (!(output & 1)) { - val = (val & 0xFF00) | input; - } else { - val = (val & 0x00FF) | (input << 8); - } - *xbar = val; -} -void xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} // function reading an ADC value and returning the read voltage @@ -178,9 +159,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); - // _UNUSED(pinC); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); +#endif if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); @@ -196,9 +178,6 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p adc1_init(pins[0], pins[1], pins[2]); - SIMPLEFOC_DEBUG("pins: ",pins[0]); - SIMPLEFOC_DEBUG("pins: ",pins[1]); - SIMPLEFOC_DEBUG("pins: ",pins[2]); adc_etc_init(pins[0], pins[1], pins[2]); xbar_init(); @@ -223,12 +202,29 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1) + //This val1 interrupt on match is in the center of the PWM + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1); + + + // if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4 // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) - flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); // setup this val4 for interrupt on match for ADC sync // this code assumes that the val4 is not used for anything else! // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center - flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center + + +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + // pin 4 observes out trigger line for 'scope + xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; + IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 + // Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO) + CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ; + // turn up drive & speed as very short pulse + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; +#endif } diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index d3738aed..bf81e246 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -13,10 +13,135 @@ #pragma message("") -// half_cycle of the PWM variable -int half_cycle = 0; + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + // function which finds the flexpwm instance for a pair of pins // if they do not belong to the same timer it returns a nullpointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ @@ -145,7 +270,7 @@ return ch2; // can configure sync, prescale and B inversion. // sets the desired frequency of the PWM // sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone ) +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) { int submodule_mask = 1 << submodule ; flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running @@ -167,17 +292,21 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque } // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); + int half_cycle = int(newdiv/2.0f); int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% int mid_pwm = int((half_cycle)/2.0f); + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + // setup the timer // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE @@ -204,17 +333,15 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int half_cycle = int(flexpwm->SM[submodule].VAL1); int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - flexpwm->SM[submodule].VAL2 = count_pwm; // A on - flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); } + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index 5e384623..a7d8c97a 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -7,6 +7,7 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { + int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; long pwm_frequency; @@ -105,6 +106,16 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; + +// find the trigger TRG0 for the given timer and submodule +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// find the external trigger for the given timer and submodule +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// function to connecting the triggers +void xbar_connect(unsigned int input, unsigned int output); +// function to initialize the xbar +void xbar_init(); + #endif #endif \ No newline at end of file From 94f4fb5675873fa6f520e380988953d832ba0488 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:38:40 +0100 Subject: [PATCH 137/220] added support for center aligned 3pwm at least for one motor + low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 16 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 277 ++++++++- .../hardware_specific/teensy/teensy4_mcu.h | 10 +- .../teensy/teensy4_mcu1.cpp.new | 543 ++++++++++++++++++ .../hardware_specific/teensy/teensy_mcu.cpp | 98 ++-- .../hardware_specific/teensy/teensy_mcu.h | 13 + 6 files changed, 896 insertions(+), 61 deletions(-) create mode 100644 src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index b76624bc..e5934920 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -2,6 +2,7 @@ #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" // #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" // if defined // - Teensy 4.0 @@ -158,7 +159,13 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // Configure low side for generic mcu // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + if(par == nullptr){ + SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); #ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); @@ -191,10 +198,15 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p // sync driver and the adc void _driverSyncLowSide(void* driver_params, void* cs_params){ - Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; + SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!"); + char buff[50]; + sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule); + SIMPLEFOC_DEBUG(buff); + // find the xbar trigger for the flexpwm int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index bf81e246..6815c10f 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,4 +1,3 @@ -#include "teensy_mcu.h" #include "teensy4_mcu.h" #include "../../../communication/SimpleFOCDebug.h" @@ -7,11 +6,11 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - #pragma message("") #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") +// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -41,7 +40,7 @@ int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; + if(submodule < 0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; }else if(flexpwm == &IMXRT_FLEXPWM2){ @@ -54,6 +53,15 @@ int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm){ + if(flexpwm == &IMXRT_FLEXPWM1) return 1; + if(flexpwm == &IMXRT_FLEXPWM2) return 2; + if(flexpwm == &IMXRT_FLEXPWM3) return 3; + if(flexpwm == &IMXRT_FLEXPWM4) return 4; + return -1; +} + // The i.MXRT1062 uses one config register per two XBAR outputs, so a helper // function to make code more readable. void xbar_connect(unsigned int input, unsigned int output) @@ -223,11 +231,34 @@ int get_submodule(uint8_t pin, uint8_t pin1){ } +// function which finds the channel for flexpwm timer pin +// 0 - X +// 1 - A +// 2 - B +int get_channel(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + info = pwm_pin_info + pin; +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); + SIMPLEFOC_DEBUG(s); +#endif + return info->channel; +} + // function which finds the timer submodule for a pair of pins // if they do not belong to the same submodule it returns a -1 int get_inverted_channel(uint8_t pin, uint8_t pin1){ - const struct pwm_pin_info_struct *info; if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; @@ -237,10 +268,8 @@ int get_inverted_channel(uint8_t pin, uint8_t pin1){ return -1; } - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); if (ch1 != 1) { #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -320,6 +349,70 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, c } +// Helper to set up a FlexPWM submodule. +// can configure sync, prescale +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_timer_submodule (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + int half_cycle = int(newdiv/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = 0 ; + flexpwm->SM[submodule].DTCNT1 = 0 ; + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, int channel) +{ + int submodule_mask = 1 << submodule ; + + if(channel==1) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + else if(channel==2) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + // staring the PWM on A and B channels of the submodule void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) { @@ -352,6 +445,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; int submoduleA,submoduleB,submoduleC; int inverted_channelA,inverted_channelB,inverted_channelC; + int channelA,channelB,channelC; flexpwmA = get_flexpwm(pinA_h,pinA_l); submoduleA = get_submodule(pinA_h,pinA_l); inverted_channelA = get_inverted_channel(pinA_h,pinA_l); @@ -361,6 +455,9 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons flexpwmC = get_flexpwm(pinC_h,pinC_l); submoduleC = get_submodule(pinC_h,pinC_l); inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + channelA = get_channel(pinA_h); + channelB = get_channel(pinB_h); + channelC = get_channel(pinC_h); if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -381,21 +478,18 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons return SIMPLEFOC_DRIVER_INIT_FAILED; } - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-DRV: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-DRV: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // others externally synced setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; // this is the master, internally synced delayMicroseconds (100) ; // turn on XBAR1 clock for all but stop mode @@ -409,7 +503,6 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons startup_pwm_pair (flexpwmB, submoduleB) ; startup_pwm_pair (flexpwmC, submoduleC) ; - delayMicroseconds(50) ; // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; @@ -419,6 +512,17 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {1,2, 1, 2, 1, 2}, + .dead_zone = dead_zone + } + }; return params; } @@ -428,11 +532,136 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // - Stepper motor - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); + write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a); + write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b); + write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c); +} + +void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty) +{ + uint16_t mask = 1 << submodule; + uint32_t half_cycle = p->SM[submodule].VAL1; + int mid_pwm = int((half_cycle)/2.0f); + int cval = int(mid_pwm*(duty*2-1)) + mid_pwm; + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = half_cycle - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL2 = -cval; + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL4 = -cval; + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } +#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configureCenterAligned3PMW(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + int channelA = get_channel(pinA); + int channelB = get_channel(pinB); + int channelC = get_channel(pinC); + + + // if pins belong to the flextimers and they only use submodules A and B + // we can configure the center-aligned pwm + if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + #endif + + // Configure FlexPWM units + setup_pwm_timer_submodule (flexpwmA, submoduleA, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmB, submoduleB, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmC, submoduleC, false, pwm_frequency) ; // this is the master, internally synced + delayMicroseconds (100) ; + + + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-CS: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-CS: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif + + // // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {channelA, channelB, channelC}, + } + }; + + startup_pwm_pair (flexpwmA, submoduleA, channelA) ; + startup_pwm_pair (flexpwmB, submoduleB, channelB) ; + startup_pwm_pair (flexpwmC, submoduleC, channelC) ; + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; + }else{ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + #endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + write_pwm_on_pin (p->flextimers[0], p->submodules[0], p->channels[0], dc_a); + write_pwm_on_pin (p->flextimers[1], p->submodules[1], p->channels[1], dc_b); + write_pwm_on_pin (p->flextimers[2], p->submodules[2], p->channels[2], dc_c); +} + +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index a7d8c97a..aed64826 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + #include "teensy_mcu.h" // if defined @@ -7,10 +10,9 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { - int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; - long pwm_frequency; + int channels[6]; float dead_zone; } Teensy4DriverParams; @@ -106,7 +108,8 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; - +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm); // find the trigger TRG0 for the given timer and submodule int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); // find the external trigger for the given timer and submodule @@ -118,4 +121,5 @@ void xbar_init(); #endif +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new new file mode 100644 index 00000000..5dcac90d --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new @@ -0,0 +1,543 @@ +#include "teensy_mcu.h" +#include "teensy4_mcu.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// half_cycle of the PWM variable +int half_cycle = 0; + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} + + +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + +// function which finds the flexpwm instance for a pair of pins +// if they do not belong to the same timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; + switch ((info->module >> 4) & 3) { + case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; + default: flexpwm1 = &IMXRT_FLEXPWM4; + } + + info = pwm_pin_info + pin1; + switch ((info->module >> 4) & 3) { + case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; + default: flexpwm2 = &IMXRT_FLEXPWM4; + } + if(flexpwm1 == flexpwm2){ + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); + return flexpwm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_submodule(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + info = pwm_pin_info + pin1; + int sm2 = info->module&0x3; + + if (sm1 == sm2) { + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); + SIMPLEFOC_DEBUG(s); + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_inverted_channel(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int ch1 = info->channel; + info = pwm_pin_info + pin1; + int ch2 = info->channel; + + if (ch1 != 1) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else if (ch2 != 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); + SIMPLEFOC_DEBUG(s); +#endif +return ch2; + } +} + +// Helper to set up A/B pair on a FlexPWM submodule. +// can configure sync, prescale and B inversion. +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + half_cycle = int(newdiv/2.0f); + int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% + int mid_pwm = int((half_cycle)/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle ; + flexpwm->SM[submodule].VAL2 = -mid_pwm ; + flexpwm->SM[submodule].VAL3 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) +{ + int submodule_mask = 1 << submodule ; + + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + +// PWM setting on the high and low pair of the PWM channels +void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int mid_pwm = int((half_cycle)/2.0f); + int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; + + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + int count_pwm = int(count*duty); + + SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); + SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); + SIMPLEFOC_DEBUG("count: ",count_pwm); + + // flexpwm->SM[submodule].VAL1 = 0; // A on + flexpwm->SM[submodule].VAL2 = count_pwm ; // A off + flexpwm->SM[submodule].VAL3 = count_pwm; // A on + flexpwm->SM[submodule].VAL4 = count_pwm ; // A off + flexpwm->SM[submodule].VAL5 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; + if (cval > modulo) cval = modulo; // TODO: is this check correct? + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = modulo - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 6PWM setting +// - hardware specific +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) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA_h,pinA_l); + submoduleA = get_submodule(pinA_h,pinA_l); + inverted_channelA = get_inverted_channel(pinA_h,pinA_l); + flexpwmB = get_flexpwm(pinB_h,pinB_l); + submoduleB = get_submodule(pinB_h,pinB_l); + inverted_channelB = get_inverted_channel(pinB_h,pinB_l); + flexpwmC = get_flexpwm(pinC_h,pinC_l); + submoduleC = get_submodule(pinC_h,pinC_l); + inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + + if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + + // Configure FlexPWM units, each driving A/B pair, B inverted. + // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + delayMicroseconds (100) ; + + // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + + delayMicroseconds(50) ; + // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; + *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; + *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; + *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; + *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; + *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + return params; +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA, pinB, pinC }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + }; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + // analogWriteFrequency(pinA, pwm_frequency); + // analogWriteFrequency(pinB, pwm_frequency); + // analogWriteFrequency(pinC, pwm_frequency); + // analogWrite(pinA, 0); + // analogWrite(pinB, 0); + // analogWrite(pinC, 0); + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index dcfa3e15..2ad5f57c 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -2,6 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#include "../../../communication/SimpleFOCDebug.h" + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); @@ -11,14 +13,15 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -26,38 +29,53 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm +__attribute__((weak)) void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency - }; + + // try configuring center aligned pwm + void* p = _configureCenterAligned3PMW(pwm_frequency, pinA, pinB, pinC); + if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ + return p; // if center aligned pwm is available return the params + }else{ // if center aligned pwm is not available use fast pwm + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; return params; + } } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -65,9 +83,10 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB, pinC, pinD }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -75,42 +94,57 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_b); +} + +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm implementation of this function +__attribute__((weak)) void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(params); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); + + TeensyDriverParams* p = (TeensyDriverParams*)params; + if(p->additional_params != nullptr){ + _writeCenterAligned3PMW(dc_a, dc_b, dc_c, p); + }else{ + // transform duty cycle from [0,1] to [0,255] + analogWrite(p->pins[0], 255.0f*dc_a); + analogWrite(p->pins[1], 255.0f*dc_b); + analogWrite(p->pins[2], 255.0f*dc_c); + } } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); - analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((TeensyDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((TeensyDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.h b/src/drivers/hardware_specific/teensy/teensy_mcu.h index 7956ea90..266f4b69 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + #include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +11,17 @@ // debugging output #define SIMPLEFOC_TEENSY_DEBUG +typedef struct TeensyDriverParams { + int pins[6] = {(int)NOT_SET}; + long pwm_frequency; + void* additional_params; +} TeensyDriverParams; + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin); +void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params); + +#endif #endif \ No newline at end of file From 238d26b63fb88fe77042055bb410a387f18f9a49 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:54:32 +0100 Subject: [PATCH 138/220] better error messages --- .../hardware_specific/teensy/teensy4_mcu.cpp | 15 +++++++-------- .../hardware_specific/teensy/teensy_mcu.cpp | 1 + 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 6815c10f..0e7953ff 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -87,7 +87,8 @@ void xbar_init() { IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2) { #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); @@ -95,7 +96,6 @@ IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ #endif return nullptr; } - info = pwm_pin_info + pin; // FlexPWM pin IMXRT_FLEXPWM_t *flexpwm; switch ((info->module >> 4) & 3) { @@ -236,17 +236,16 @@ int get_submodule(uint8_t pin, uint8_t pin1){ // 1 - A // 2 - B int get_channel(uint8_t pin){ - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2){ #ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; + char s[90]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); SIMPLEFOC_DEBUG(s); #endif return -1; } - info = pwm_pin_info + pin; #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); @@ -598,7 +597,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe // we can configure the center-aligned pwm if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B channels - Configuring center-aligned pwm!"); #endif // Configure FlexPWM units @@ -645,7 +644,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe return params; }else{ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B channels - cannot configure center-aligned pwm!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index 2ad5f57c..196f07fd 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -61,6 +61,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ return p; // if center aligned pwm is available return the params }else{ // if center aligned pwm is not available use fast pwm + SIMPLEFOC_DEBUG("TEENSY-DRV: Configuring 3PWM with fast pwm. Please consider using center aligned pwm for better performance!"); _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); From dec7abf63c460340f34bf9b48d2ef1f5530b6d21 Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 10 Mar 2024 20:30:45 -0400 Subject: [PATCH 139/220] added code to set timer prescalers and overflow based on actual timer clock frequency --- .../hardware_specific/stm32/stm32_mcu.cpp | 47 +++++++++++++++++-- .../hardware_specific/stm32/stm32_mcu.h | 3 ++ 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 33b5b664..9a349670 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -26,7 +26,15 @@ PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; +bool _getPwmState(void* params) { + // assume timers are synchronized and that there's at least one timer + HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; + TIM_HandleTypeDef* htim = pHT->getHandle(); + + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); + return dir; +} // setting pwm to hardware pin - instead analogWrite() @@ -230,7 +238,7 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // #endif return -1; } -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found @@ -303,6 +311,30 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { } #endif +void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { + uint32_t max_frequency = 0; + uint32_t min_frequency = UINT32_MAX; + for (size_t i=0; igetTimerClkFreq(); + if (freq > max_frequency) { + max_frequency = freq; + } else if (freq < min_frequency) { + min_frequency = freq; + } + } + if (max_frequency==min_frequency) return; + uint32_t overflow_value = min_frequency/pwm_frequency; + for (size_t i=0; igetTimerClkFreq()/min_frequency; + #ifdef SIMPLEFOC_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); + SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); + #endif + timers[i]->setPrescaleFactor(prescale_factor); + timers[i]->setOverflow(overflow_value,TICK_FORMAT); + timers[i]->refresh(); + } +} void _alignTimersNew() { int numTimers = 0; @@ -382,7 +414,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - //timers[i]->refresh(); + timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -735,6 +767,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer *timers[2] = {HT1, HT2}; + syncTimerFrequency(pwm_frequency, timers, 2); // allign the timers _alignPWMTimers(HT1, HT2, HT2); @@ -779,6 +813,9 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); + HardwareTimer *timers[3] = {HT1, HT2, HT3}; + syncTimerFrequency(pwm_frequency, timers, 3); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -821,6 +858,8 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); + HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; + syncTimerFrequency(pwm_frequency, timers, 4); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); @@ -918,6 +957,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + syncTimerFrequency(pwm_frequency, timers, 6); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -1103,4 +1144,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif -#endif +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..411c43b2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -28,5 +28,8 @@ typedef struct STM32DriverParams { void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); +// timer query functions +bool _getPwmState(void* params); + #endif #endif \ No newline at end of file From f2b7b22deae44254a2c2284ea0665cddcc8d0f8f Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 10 Mar 2024 20:45:04 -0400 Subject: [PATCH 140/220] comment out HT->refresh() call --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 9a349670..0fdec2f0 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -414,7 +414,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + // timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -1144,4 +1144,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif -#endif \ No newline at end of file +#endif From 49da7b0630b6af9ed254bc570f8d3795825ae402 Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 17 Mar 2024 16:20:38 -0400 Subject: [PATCH 141/220] add f7 current sense files --- .../stm32/stm32f7/stm32f7_hal.cpp | 185 +++++++++++++ .../stm32/stm32f7/stm32f7_hal.h | 15 ++ .../stm32/stm32f7/stm32f7_mcu.cpp | 111 ++++++++ .../stm32/stm32f7/stm32f7_utils.cpp | 255 ++++++++++++++++++ .../stm32/stm32f7/stm32f7_utils.h | 30 +++ 5 files changed, 596 insertions(+) create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp new file mode 100644 index 00000000..d4cffec6 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -0,0 +1,185 @@ +#include "stm32f7_hal.h" + +#if defined(STM32F7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +ADC_HandleTypeDef hadc; + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + // check if all pins belong to the same ADC + ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; + if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); +#endif + return -1; + } + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef ADC3 // if defined ADC3 + else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE(); +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 1; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; + + // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; + // if(TRGO_already_configured) continue; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = timer_to_check; + if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) { + // workaround for errata 2.2.1 in ES0290 Rev 7 + // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf + __HAL_RCC_DAC_CLK_ENABLE(); + } + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); + #endif + return -1; + } + // display which timer is being used + #ifdef SIMPLEFOC_STM32_DEBUG + // it would be better to use the getTimerNumber from driver + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + #endif + + + // first channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); +#endif + return -1; + } + + // second channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; +#endif + return -1; + } + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif + return -1; + } + } + + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif + + cs_params->adc_handle = &hadc; + return 0; +} + +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + uint8_t cnt = 0; + if(_isset(pinA)){ + pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); + cs_params->pins[cnt++] = pinA; + } + if(_isset(pinB)){ + pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); + cs_params->pins[cnt++] = pinB; + } + if(_isset(pinC)){ + pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); + cs_params->pins[cnt] = pinC; + } +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h new file mode 100644 index 00000000..0a3614b5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -0,0 +1,15 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) +#include "stm32f7xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32f7_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp new file mode 100644 index 00000000..3040ea46 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -0,0 +1,111 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F7xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32f7_hal.h" +#include "stm32f7_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (3) +uint32_t adc_val[3][4]={0}; +// does adc interrupt need a downsample - per adc (3) +bool needs_downsample[3] = {1}; +// downsampling variable - per adc (3) +uint8_t tim_downsample[3] = {1}; + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return; + + // stop all the timers for the driver + _stopTimers(driver_params->timers, 6); + + // if timer has repetition counter - it will downsample using it + // and it does not need the software downsample + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + // adjust the initial timer state such that the trigger + // - for DMA transfer aligns with the pwm peaks instead of throughs. + // - for interrupt based ADC transfer + // - only necessary for the timers that have repetition counters + + cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + // remember that this timer has repetition counter - no need to downasmple + needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + } + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp new file mode 100644 index 00000000..d5f8c6b2 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -0,0 +1,255 @@ +#include "stm32f7_utils.h" + +#if defined(STM32F7xx) + +/* Exported Functions */ + + +PinName analog_to_pin(uint32_t pin) { + PinName pin_name = analogInputToPinName(pin); + if (pin_name == NC) { + return (PinName) pin; + } + return pin_name; +} + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} +/* +TIM1 +TIM2 +TIM3 +TIM4 +TIM5 +TIM6 +TIM7 +TIM12 +TIM13 +TIM14 + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO +ADC_EXTERNALTRIGINJECCONV_T2_TRGO +ADC_EXTERNALTRIGINJECCONV_T4_TRGO + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T8_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T5_TRGO +ADC_EXTERNALTRIGINJECCONV_T6_TRGO +*/ +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} +/* + +ADC_EXTERNALTRIGCONV_T5_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO2 +ADC_EXTERNALTRIGCONV_T1_TRGO +ADC_EXTERNALTRIGCONV_T1_TRGO2 +ADC_EXTERNALTRIGCONV_T2_TRGO +ADC_EXTERNALTRIGCONV_T4_TRGO +ADC_EXTERNALTRIGCONV_T6_TRGO +*/ + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h new file mode 100644 index 00000000..017ff464 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -0,0 +1,30 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin); + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif \ No newline at end of file From 92cb9af1da548c9f978577327961eed6f7686a7f Mon Sep 17 00:00:00 2001 From: Yannik Stradmann <49203055+ystradmann@users.noreply.github.com> Date: Wed, 20 Mar 2024 00:18:19 +0100 Subject: [PATCH 142/220] Add missing implementation for MagneticSensorI2C::AS5600 --- src/sensors/MagneticSensorI2C.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index ac2b273b..7b4ea77e 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -58,6 +58,10 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ wire = &Wire; } +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; From d48f4d795591b688470c791509f7117bb745dd0a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:15:21 +0200 Subject: [PATCH 143/220] added support for debugging with char --- src/communication/SimpleFOCDebug.cpp | 6 ++++++ src/communication/SimpleFOCDebug.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..3bb62bce 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -58,6 +58,12 @@ void SimpleFOCDebug::println(const char* str, int val) { _debugPrint->println(val); } } +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..4fcfd538 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -45,6 +45,7 @@ class SimpleFOCDebug { static void println(const char* msg, float val); static void println(const __FlashStringHelper* msg, int val); static void println(const char* msg, int val); + static void println(const char* msg, char val); static void println(); static void println(int val); static void println(float val); From c171a019114277546f8fcb57240f67bf32d51bb3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:27:32 +0200 Subject: [PATCH 144/220] renamed the flag to force_center_aligend_3pwm --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 0e7953ff..060ff4e2 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -10,7 +10,7 @@ #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") -// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM +// #define TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -83,7 +83,7 @@ void xbar_init() { } // function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer +// if it does not belong to the flexpwm timer it returns a null-pointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM +#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting From 82da85726a59d141a96ecebd06699a3f050aa0f3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:28:52 +0200 Subject: [PATCH 145/220] small restructure of commented code --- src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index e5934920..564f4f46 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -12,9 +12,9 @@ // #define TEENSY4_ADC_INTERRUPT_DEBUG -// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense // LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); From d6a0b26d72158ba944a3089b511c960e9afe875f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:30:51 +0200 Subject: [PATCH 146/220] a small typo --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 060ff4e2..7c4d0dbb 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM +#ifdef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting From c2c4365618d2de1b7443bf9528f88184b7ca4bc0 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:06:26 +0200 Subject: [PATCH 147/220] enable disabling dowsanmpling #377 --- src/common/base_classes/FOCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..6ea26624 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -91,7 +91,7 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { - if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return; + if( monitor_cnt++ < monitor_downsample ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; From f796c9d0165520366925b70c231794e35f561d40 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:20:36 +0200 Subject: [PATCH 148/220] added the sqrt(3) factor #372 --- .../find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino | 2 +- .../hall_sensor/find_kv_rating/find_kv_rating.ino | 2 +- .../magnetic_sensor/find_kv_rating/find_kv_rating.ino | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index 76fb46b0..c304d195 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -36,7 +36,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 3abef467..575968f4 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -33,7 +33,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index f3dd74a1..a9d29d86 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -31,7 +31,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } From 96b08040bd904b79378868306df98f3e0c33e8c6 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:27:11 +0200 Subject: [PATCH 149/220] another way of enabling downsample disable #377 --- src/common/base_classes/FOCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 6ea26624..e0a00887 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -91,7 +91,7 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { - if( monitor_cnt++ < monitor_downsample ) return; + if( !monitor_downsample || monitor_cnt++ < (monitor_downsample-1) ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; From 977b1412c6a2e5d5d3061edc44941c8db6abe043 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 19:37:31 +0200 Subject: [PATCH 150/220] added support for openloop in the initFOC #382 --- src/BLDCMotor.cpp | 50 ++++++++++++++++++++++++++------------------ src/StepperMotor.cpp | 20 ++++++++++++++---- 2 files changed, 46 insertions(+), 24 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9dbc0a48..fe14feff 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -156,25 +156,31 @@ int BLDCMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else { - exit_flag = 0; // no FOC without sensor - SIMPLEFOC_DEBUG("MOT: No sensor."); - } - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - if(exit_flag){ - if(current_sense){ - if (!current_sense->initialized) { - motor_status = FOCMotorStatus::motor_calib_failed; - SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); - exit_flag = 0; - }else{ - exit_flag *= alignCurrentSense(); + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor } - else { SIMPLEFOC_DEBUG("MOT: No current sense."); } } if(exit_flag){ @@ -219,6 +225,10 @@ int BLDCMotor::alignSensor() { // stop init if not found index if(!exit_flag) return exit_flag; + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction==Direction::UNKNOWN){ @@ -226,7 +236,7 @@ int BLDCMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -236,13 +246,13 @@ int BLDCMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } sensor->update(); float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); + // setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved float moved = fabs(mid_angle - end_angle); @@ -270,7 +280,7 @@ int BLDCMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 697ed277..5d519f29 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -114,7 +114,15 @@ int StepperMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -133,6 +141,10 @@ int StepperMotor::alignSensor() { int exit_flag = 1; //success SIMPLEFOC_DEBUG("MOT: Align sensor."); + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction == Direction::UNKNOWN){ // check if sensor needs zero search @@ -144,7 +156,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -154,7 +166,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -190,7 +202,7 @@ int StepperMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); From 65e2208ec9dd446ea345aeb4f2063c679333856c Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 20:13:49 +0200 Subject: [PATCH 151/220] readme prepare for v2.3.3 --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 7d8150d0..e115ee51 100644 --- a/README.md +++ b/README.md @@ -26,10 +26,14 @@ Therefore this is an attempt to: - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 -> - And more bugfixes - see the complete list of 2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> - Teensy4 +> - support for low-side current sensing +> - support for center aligned 6pwm and 3pwm +> - stm32 +> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) -## Arduino *SimpleFOClibrary* v2.3.2 +## Arduino *SimpleFOClibrary* v2.3.3

From af2c7c0e22e7c704a9c6f1257dbff9fde43baf66 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 20:17:10 +0200 Subject: [PATCH 152/220] readme prepare for v2.3.3 v2 --- README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index e115ee51..29e715fe 100644 --- a/README.md +++ b/README.md @@ -27,10 +27,15 @@ Therefore this is an attempt to: > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 > - Teensy4 -> - support for low-side current sensing -> - support for center aligned 6pwm and 3pwm +> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) +> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) > - stm32 -> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) +> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378), +> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) +> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) +> - And much more: +> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) ## Arduino *SimpleFOClibrary* v2.3.3 From e6b3f4f65283fe0f34f49edad42020c65d51ba90 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 07:59:07 +0200 Subject: [PATCH 153/220] Update ccpp.yml --- .github/workflows/ccpp.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6c396ea9..bcb316df 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,7 +26,8 @@ jobs: - esp32:esp32:esp32s2 # esp32s2 - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -92,6 +93,10 @@ jobs: platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # nucleo one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + # Do not cancel all jobs / architectures if one job fails fail-fast: false From 1d92c3da5a6b5d2566f946124f52c55591fbc101 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:02:50 +0200 Subject: [PATCH 154/220] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index bcb316df..c19b651f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,7 +27,7 @@ jobs: - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg + - STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -93,7 +93,7 @@ jobs: platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # nucleo one full example + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # nucleo f7 one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino From be24ec7346f1bec7e4c4b2b3cc17de933e1c0705 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:09:23 +0200 Subject: [PATCH 155/220] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c19b651f..fa3cf972 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,7 +27,7 @@ jobs: - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # stm32 nucleo f746zg + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -93,7 +93,7 @@ jobs: platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # nucleo f7 one full example + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino From e8421ed2f001ad6eb8064fbecd9f0cb9142eb0b3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:23:14 +0200 Subject: [PATCH 156/220] added teesny low-side example, and complted the readme with f7 --- README.md | 11 +- .../teensy4_current_control_low_side.ino | 170 ++++++++++++++++++ 2 files changed, 175 insertions(+), 6 deletions(-) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino diff --git a/README.md b/README.md index 29e715fe..5b311224 100644 --- a/README.md +++ b/README.md @@ -19,11 +19,9 @@ Additionally, most of the efforts at this moment are still channeled towards the Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there -- 🎯 Develop a modular FOC supporting BLDC driver boards: - - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini). - - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) +- 🎯 Develop modular and easy to use FOC supporting BLDC driver boards + - For official driver boards see [SimpleFOCBoards](boards) + - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 > - Teensy4 @@ -31,7 +29,8 @@ Therefore this is an attempt to: > - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) > - stm32 > - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) -> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378), +> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378) +> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394) > - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) > - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) > - And much more: diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino new file mode 100644 index 00000000..67fec8a1 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -0,0 +1,170 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define EN_GATE 11 +#define M_PWM 22 +#define GAIN 20 +#define M_OC 23 +#define OC_ADJ 19 + +#define INH_A 2 +#define INL_A 3 +#define INH_B 8 +#define INL_B 7 +#define INH_C 6 +#define INL_C 9 + +#define IOUTA 14 +#define IOUTB 15 +#define IOUTC 16 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB); + +// encoder instance +Encoder encoder = Encoder(10, 11, 2048); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 6pwm mode + pinMode(M_PWM, OUTPUT); + digitalWrite(M_PWM,LOW); // high for 3pwm + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 19; + driver.pwm_frequency = 20000; // suggested not higher than 22khz + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 0; + + // initialise motor + motor.init(); + + cs.init(); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file From 719279147bf92ff1a1b0b7e693dd4c1cc6a33244 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:24:34 +0200 Subject: [PATCH 157/220] typo in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5b311224..3323a794 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop modular and easy to use FOC supporting BLDC driver boards - - For official driver boards see [SimpleFOCBoards](boards) + - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 From 4fa7cf2b0fccb36abcd318c8c20d19b69bf182fa Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:27:11 +0200 Subject: [PATCH 158/220] Update teensy.yml --- .github/workflows/teensy.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index 9fc88b9a..f1f89185 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -29,6 +29,11 @@ jobs: run: pio ci --lib="." --board=teensy41 --board=teensy40 env: PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino - name: PIO Run Teensy 3 run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 From cc6dfa06fac30db7f1727040dd7635689e694115 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:31:19 +0200 Subject: [PATCH 159/220] typo in the example --- .../teensy4_current_control_low_side.ino | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino index 67fec8a1..841134d8 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -71,8 +71,6 @@ void setup() { // Better option would be to use voltage divisor to set exact value pinMode(OC_ADJ,OUTPUT); digitalWrite(OC_ADJ,HIGH); - pinMode(OC_GAIN,OUTPUT); - digitalWrite(OC_GAIN,LOW); // driver config From 4d33cd79eaa99b2e3035cbe9669cc5661b3a1c11 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:37:51 +0200 Subject: [PATCH 160/220] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6c396ea9..0ffbed91 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -36,7 +36,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 3942e88e09592a37dd21d426769ba6e32a04d97d Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:45:26 +0200 Subject: [PATCH 161/220] remove teensy sketch form uno --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index fa3cf972..f870e780 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -37,7 +37,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From c54623de5efd12f8c8f958146222564d1675dff9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:08:28 +0200 Subject: [PATCH 162/220] restructured the CI --- .github/workflows/arduino.yml | 53 +++++++++++++++++++++++ .github/workflows/esp32.yml | 50 +++++++++++++++++++++ .github/workflows/rpi.yml | 39 +++++++++++++++++ .github/workflows/samd.yml | 44 +++++++++++++++++++ .github/workflows/{ccpp.yml => stm32.yml} | 50 +-------------------- .github/workflows/teensy.yml | 2 +- README.md | 7 ++- 7 files changed, 195 insertions(+), 50 deletions(-) create mode 100644 .github/workflows/arduino.yml create mode 100644 .github/workflows/esp32.yml create mode 100644 .github/workflows/rpi.yml create mode 100644 .github/workflows/samd.yml rename .github/workflows/{ccpp.yml => stm32.yml} (52%) diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml new file mode 100644 index 00000000..639526ac --- /dev/null +++ b/.github/workflows/arduino.yml @@ -0,0 +1,53 @@ +name: AVR build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:avr:mega # arduino mega2650 + - arduino:avr:leonardo # arduino leonardo + + include: + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples + sketch-names: '**.ino' + required-libraries: PciManager + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example + sketch-names: single_full_control_example.ino + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml new file mode 100644 index 00000000..12f8e3b8 --- /dev/null +++ b/.github/workflows/esp32.yml @@ -0,0 +1,50 @@ +name: ESP32 build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - esp32:esp32:esp32doit-devkit-v1 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 + + include: + + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino + + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml new file mode 100644 index 00000000..ac18a920 --- /dev/null +++ b/.github/workflows/rpi.yml @@ -0,0 +1,39 @@ +name: RP2040 build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:mbed_rp2040:pico # rpi pico + + include: + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example + sketch-names: open_loop_position_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml new file mode 100644 index 00000000..a31c0788 --- /dev/null +++ b/.github/workflows/samd.yml @@ -0,0 +1,44 @@ +name: SAMD build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + + include: + + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + sketch-names: single_full_control_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/ccpp.yml b/.github/workflows/stm32.yml similarity index 52% rename from .github/workflows/ccpp.yml rename to .github/workflows/stm32.yml index f870e780..50224a1f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/stm32.yml @@ -1,4 +1,4 @@ -name: Library Compile +name: STM32 build on: [push, pull_request] jobs: lint: @@ -16,60 +16,14 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:avr:uno # arudino uno - - arduino:sam:arduino_due_x # arduino due - - arduino:avr:mega # arduino mega2650 - - arduino:avr:leonardo # arduino leonardo - - arduino:samd:nano_33_iot # samd21 - - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esp32 - - esp32:esp32:esp32s2 # esp32s2 - - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - - arduino:mbed_rp2040:pico # rpi pico - - include: - - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples - sketch-names: '**.ino' - required-libraries: PciManager - sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - - - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 - sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - - - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example - platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + include: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index f1f89185..e126c616 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -1,4 +1,4 @@ -name: PlatformIO - Teensy build +name: Teensy build on: [push] diff --git a/README.md b/README.md index 3323a794..adbdd09a 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,12 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) +![AVR](https://github.com/simplefoc/Arduino-FOC/workflows/AVR%20build/badge.svg) +![ESP32](https://github.com/simplefoc/Arduino-FOC/workflows/ESP32%20build/badge.svg) +![STM32](https://github.com/simplefoc/Arduino-FOC/workflows/STM32%20build/badge.svg) +![RP2040](https://github.com/simplefoc/Arduino-FOC/workflows/RP2040%20build/badge.svg) +![SAMD](https://github.com/simplefoc/Arduino-FOC/workflows/SAMD%20build/badge.svg) +[![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) From 5c9e32a28b50597b9cc5051caab5cfef7a0f4f48 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:13:07 +0200 Subject: [PATCH 163/220] added links to the new CI to readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index adbdd09a..86551f79 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,11 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![AVR](https://github.com/simplefoc/Arduino-FOC/workflows/AVR%20build/badge.svg) -![ESP32](https://github.com/simplefoc/Arduino-FOC/workflows/ESP32%20build/badge.svg) -![STM32](https://github.com/simplefoc/Arduino-FOC/workflows/STM32%20build/badge.svg) -![RP2040](https://github.com/simplefoc/Arduino-FOC/workflows/RP2040%20build/badge.svg) -![SAMD](https://github.com/simplefoc/Arduino-FOC/workflows/SAMD%20build/badge.svg) +[![AVR build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml) +[![STM32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml) +[![ESP32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml) +[![RP2040 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml) +[![SAMD build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml) [![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) From 26c631d5a48aad09025211cd917e821f1094de0e Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:19:37 +0200 Subject: [PATCH 164/220] renamed the actions --- .github/workflows/arduino.yml | 2 +- .github/workflows/esp32.yml | 2 +- .github/workflows/rpi.yml | 2 +- .github/workflows/samd.yml | 2 +- .github/workflows/stm32.yml | 2 +- .github/workflows/teensy.yml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml index 639526ac..cadd8ccc 100644 --- a/.github/workflows/arduino.yml +++ b/.github/workflows/arduino.yml @@ -1,4 +1,4 @@ -name: AVR build +name: AVR on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index 12f8e3b8..e0899bf1 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -1,4 +1,4 @@ -name: ESP32 build +name: ESP32 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml index ac18a920..d6d72b95 100644 --- a/.github/workflows/rpi.yml +++ b/.github/workflows/rpi.yml @@ -1,4 +1,4 @@ -name: RP2040 build +name: RP2040 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml index a31c0788..c4329869 100644 --- a/.github/workflows/samd.yml +++ b/.github/workflows/samd.yml @@ -1,4 +1,4 @@ -name: SAMD build +name: SAMD on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/stm32.yml b/.github/workflows/stm32.yml index 50224a1f..52b5cc94 100644 --- a/.github/workflows/stm32.yml +++ b/.github/workflows/stm32.yml @@ -1,4 +1,4 @@ -name: STM32 build +name: STM32 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index e126c616..6ad953fe 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -1,4 +1,4 @@ -name: Teensy build +name: Teensy on: [push] From abda7543c78006cde7ae84fc76678bd4a56c1ca2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 16:54:28 +0200 Subject: [PATCH 165/220] Update teensy4_mcu.cpp --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 7c4d0dbb..47108447 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -10,7 +10,7 @@ #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") -// #define TEENSY4_FORCE_CENTER_ALIGNED_3PWM +// #define SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifdef TEENSY4_FORCE_CENTER_ALIGNED_3PWM +#ifdef SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -663,4 +663,4 @@ void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ #endif -#endif \ No newline at end of file +#endif From 6fafa35b73e9c5ba6f5d9259640a5ed328dc2750 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 16:55:11 +0200 Subject: [PATCH 166/220] Update teensy4_mcu.cpp --- .../hardware_specific/teensy/teensy4_mcu.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 564f4f46..70815d0f 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -9,7 +9,7 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// #define TEENSY4_ADC_INTERRUPT_DEBUG +// #define SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG volatile uint32_t val0, val1, val2; @@ -28,7 +28,7 @@ void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); #endif // page 3509 , section 66.5.1.3.3 @@ -37,21 +37,21 @@ void adcetc0_isr() { val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); #endif } void adcetc1_isr() { -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); #endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; // val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); #endif } @@ -167,7 +167,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); #endif @@ -228,7 +228,7 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG // pin 4 observes out trigger line for 'scope xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 @@ -241,4 +241,4 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ } -#endif \ No newline at end of file +#endif From 14f67265a2a73b985afc83fab8b036b2f295fd06 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 30 Apr 2024 12:05:09 +0200 Subject: [PATCH 167/220] fix RP2040 compile problems with earlehillpower --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index f187fbef..6afbf345 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -13,13 +13,16 @@ #pragma message("SimpleFOC: compiling for RP2040") #pragma message("") - +#if !defined(SIMPLEFOC_DEBUG_RP2040) #define SIMPLEFOC_DEBUG_RP2040 +#endif #include "../../hardware_api.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#if defined(USE_ARDUINO_PINOUT) #include +#endif #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -35,7 +38,11 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + #if defined(USE_ARDUINO_PINOUT) uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? + #else + uint pin = (uint)pin_nr; + #endif gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); From 3b7fa90b1e39d033138cc5bcd5a2140456957c5d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:01:31 +0200 Subject: [PATCH 168/220] added initial support for current sensing for stepper motors --- src/BLDCMotor.h | 1 + src/StepperMotor.cpp | 146 ++++++++++--- src/StepperMotor.h | 2 + src/common/base_classes/BLDCDriver.h | 32 +-- src/common/base_classes/CurrentSense.cpp | 253 +++++++++++++++++++++- src/common/base_classes/CurrentSense.h | 56 ++++- src/common/base_classes/FOCDriver.h | 47 ++++ src/common/base_classes/StepperDriver.h | 32 ++- src/common/foc_utils.h | 2 + src/communication/SimpleFOCDebug.h | 5 +- src/current_sense/GenericCurrentSense.cpp | 100 --------- src/current_sense/InlineCurrentSense.cpp | 177 +-------------- src/current_sense/InlineCurrentSense.h | 25 +-- src/current_sense/LowsideCurrentSense.cpp | 172 +-------------- src/current_sense/LowsideCurrentSense.h | 23 +- src/drivers/BLDCDriver3PWM.h | 6 +- src/drivers/BLDCDriver6PWM.h | 1 - src/drivers/StepperDriver2PWM.cpp | 16 +- src/drivers/StepperDriver2PWM.h | 9 + src/drivers/StepperDriver4PWM.cpp | 17 +- src/drivers/StepperDriver4PWM.h | 10 + 21 files changed, 550 insertions(+), 582 deletions(-) create mode 100644 src/common/base_classes/FOCDriver.h diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a7155c1f..41a5a1c0 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" +#include "common/base_classes/FOCDriver.h" #include "common/base_classes/BLDCDriver.h" #include "common/foc_utils.h" #include "common/time_utils.h" diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5d519f29..1e4e343c 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -108,12 +108,28 @@ int StepperMotor::initFOC() { // alignment necessary for encoders! // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); - shaft_angle = sensor->getAngle(); + shaft_angle = shaftAngle(); + + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + } else { SIMPLEFOC_DEBUG("MOT: No sensor."); if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ @@ -136,6 +152,26 @@ int StepperMotor::initFOC() { return exit_flag; } +// Calibrate the motor and current sense phases +int StepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success @@ -261,8 +297,6 @@ void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - shaft_angle = shaftAngle(); // if disabled do nothing if(!enabled) return; @@ -272,6 +306,44 @@ void StepperMotor::loopFOC() { // which is in range 0-2PI electrical_angle = electricalAngle(); + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -309,56 +381,70 @@ void StepperMotor::move(float new_target) { // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; - // choose control loop + // upgrade the current based voltage limit switch (controller) { case MotionControlType::torque: - if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); - shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit); - // calculate the torque command + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control // if torque controlled through voltage control - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; } } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 45d20c63..f76e7bcf 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroSearch(); + /** Current sense and motor phase alignment */ + int alignCurrentSense(); // Open loop motion control /** diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index f405d785..6ae8186f 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -2,40 +2,17 @@ #define BLDCDRIVER_H #include "Arduino.h" +#include "FOCDriver.h" - -enum PhaseState : uint8_t { - PHASE_OFF = 0, // both sides of the phase are off - PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode - PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) - PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) -}; - - -class BLDCDriver{ +class BLDCDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -51,6 +28,9 @@ class BLDCDriver{ * @param sa - phase C state : active / disabled ( high impedance ) */ virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::BLDC; }; }; #endif diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 03ea19ea..4e6c3386 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -1,4 +1,5 @@ #include "CurrentSense.h" +#include "../../communication/SimpleFOCDebug.h" // get current magnitude @@ -27,7 +28,7 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents // - using getPhaseCurrents and getABCurrents internally @@ -44,11 +45,21 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ return return_current; } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating Alpha Beta currents from phase currents // - function calculating Clarke transform of the phase currents ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // check if driver is an instance of StepperDriver + // if so there is no need to Clarke transform + if (driver_type == DriverType::Stepper){ + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + return return_ABcurrent; + } + + // otherwise it's a BLDC motor and // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -80,7 +91,7 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating D and Q currents from Alpha Beta currents and electrical angle // - function calculating Clarke transform of the phase currents DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ @@ -97,8 +108,10 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ /** Driver linking to the current sense */ -void CurrentSense::linkDriver(BLDCDriver* _driver) { - driver = _driver; +void CurrentSense::linkDriver(FOCDriver* _driver) { + driver = _driver; + // save the driver type for easier access + driver_type = driver->type(); } @@ -108,4 +121,232 @@ void CurrentSense::enable(){ void CurrentSense::disable(){ // nothing is done here, but you can override this function -}; \ No newline at end of file +}; + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +// IMPORTANT, this function can be overriden in the child class +int CurrentSense::driverAlign(float voltage){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + if (!initialized) return 0; + + // check if stepper or BLDC + if(driver_type == DriverType::Stepper) + return alignStepperDriver(voltage, (StepperDriver*)driver); + else + return alignBLDCDriver(voltage, (BLDCDriver*)driver); +} + + + +// Helper function to read and average phase currents +PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { + PhaseCurrent_s c = getPhaseCurrents(); + for (int i = 0; i < N; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a * 0.6f + 0.4f * c1.a; + c.b = c.b * 0.6f + 0.4f * c1.b; + c.c = c.c * 0.6f + 0.4f * c1.c; + _delay(3); + } + return c; +}; + + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ + + int exit_flag = 1; + if(_isset(pinA)){ + // set phase A active and phases B and C down + bldc_driver->setPwm(voltage, 0, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = c.b ? fabs(c.a / c.b) : 0; + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Err read A"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + if(_isset(pinB)){ + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + float ba_ratio = c.a ? fabs(c.b / c.a) : 0; + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2); + gain_b *= _sign(c.b); + }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch B-A"); + // switch phase A and B + _swap(pinB, pinA); + _swap(offset_ib, offset_ia); + _swap(gain_b, gain_a); + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Error read B"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + // if phase C measured + if(_isset(pinC)){ + // set phase C active and phases A and B down + bldc_driver->setPwm(0, 0, voltage); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + float ca_ratio = c.a ? fabs(c.c / c.a) : 0; + float cb_ratio = c.b ? fabs(c.c / c.b) : 0; + if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch C-A"); + // switch phase A and C + _swap(pinC, pinA); + _swap(offset_ic, offset_ia); + _swap(gain_c, gain_a); + gain_c *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch C-B"); + _swap(pinC, pinB); + _swap(offset_ic, offset_ib); + _swap(gain_c, gain_b); + gain_b *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Err read C"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + // add 2 if pin gains negative + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + return exit_flag; +} + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ + + int exit_flag = 1; + + if(_isset(pinA)){ + // set phase A active to high and B to low + stepper_driver->setPwm(voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // check if measured current a is positive and invert if not + // check if current b is around zero and if its not + // check if current a is near zero and if it is invert them + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Neg A"); + gain_a *= -1; + } + } + + if(_isset(pinB)){ + // set phase B active and phases A and C down + stepper_driver->setPwm(voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Neg B"); + gain_b *= -1; + } + } + + // add 2 if pin gains negative + if(gain_a < 0 || gain_b < 0 ) exit_flag +=2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 1c839053..c9c79650 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,12 +1,15 @@ #ifndef CURRENTSENSE_H #define CURRENTSENSE_H -#include "BLDCDriver.h" +#include "FOCDriver.h" #include "../foc_utils.h" +#include "../time_utils.h" +#include "StepperDriver.h" +#include "BLDCDriver.h" /** * Current sensing abstract class defintion - * Each current sensoring implementation needs to extend this interface + * Each current sensing implementation needs to extend this interface */ class CurrentSense{ public: @@ -23,24 +26,49 @@ class CurrentSense{ * Linking the current sense with the motor driver * Only necessary if synchronisation in between the two is required */ - void linkDriver(BLDCDriver *driver); + void linkDriver(FOCDriver *driver); // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver = nullptr; //!< driver link + FOCDriver* driver = nullptr; //!< driver link bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing + DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper) + + // ADC measurement gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + /** * Function intended to verify if: * - phase current are oriented properly * - if their order is the same as driver phases * * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & positive number (with status) - for success + * @returns - + 0 - failure + 1 - success and nothing changed + 2 - success but pins reconfigured + 3 - success but gains inverted + 4 - success but pins reconfigured and gains inverted + * + * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage) = 0; + virtual int driverAlign(float align_voltage); /** * Function rading the phase currents a, b and c @@ -80,7 +108,7 @@ class CurrentSense{ /** * Function used for Park transform in FOC control - * It reads the Alpha Beta currents and electircal angle of the motor + * It reads the Alpha Beta currents and electrical angle of the motor * It returns the D and Q currents * * @param current - phase current @@ -98,6 +126,20 @@ class CurrentSense{ * override it to do something useful. */ virtual void disable(); + + /** + * Function used to align the current sense with the BLDC motor driver + */ + int alignBLDCDriver(float align_voltage, BLDCDriver* driver); + /** + * Function used to align the current sense with the Stepper motor driver + */ + int alignStepperDriver(float align_voltage, StepperDriver* driver); + /** + * Function used to read the average current values over N samples + */ + PhaseCurrent_s readAverageCurrents(int N=100); + }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h new file mode 100644 index 00000000..944251a4 --- /dev/null +++ b/src/common/base_classes/FOCDriver.h @@ -0,0 +1,47 @@ +#ifndef FOCDRIVER_H +#define FOCDRIVER_H + +#include "Arduino.h" + + +enum PhaseState : uint8_t { + PHASE_OFF = 0, // both sides of the phase are off + PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode + PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) + PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) +}; + + +enum DriverType{ + Unknown=0, + BLDC=1, + Stepper=2 +}; + +/** + * FOC driver class + */ +class FOCDriver{ + public: + + /** Initialise hardware */ + virtual int init() = 0; + /** Enable hardware */ + virtual void enable() = 0; + /** Disable hardware */ + virtual void disable() = 0; + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + bool initialized = false; //!< true if driver was successfully initialized + void* params = 0; //!< pointer to hardware specific parameters of driver + + bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) + + /** get the driver type*/ + virtual DriverType type() = 0; +}; + +#endif diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..9864b235 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,32 +1,30 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H -#include "drivers/hardware_api.h" +#include "Arduino.h" +#include "FOCDriver.h" -class StepperDriver{ +class StepperDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua phase A voltage * @param Ub phase B voltage */ virtual void setPwm(float Ua, float Ub) = 0; + + /** + * Set phase state, enable/disable + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::Stepper; } ; }; #endif \ No newline at end of file diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index f2bc9ef6..9a5b53ea 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -14,6 +14,8 @@ #define _UNUSED(v) (void) (v) #define _powtwo(x) (1 << (x)) +#define _swap(a, b) { auto temp = a; a = b; b = temp; } + // utility defines #define _2_SQRT3 1.15470053838f #define _SQRT3 1.73205080757f diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 4fcfd538..93686650 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -32,6 +32,7 @@ * **/ +// #define SIMPLEFOC_DISABLE_DEBUG #ifndef SIMPLEFOC_DISABLE_DEBUG @@ -63,10 +64,6 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) - - - - #else //ifndef SIMPLEFOC_DISABLE_DEBUG diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 635535fa..9d90f0ca 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,110 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted int GenericCurrentSense::driverAlign(float voltage){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; - if (!initialized) return 0; - - // // set phase A active and phases B and C down - // driver->setPwm(voltage, 0, 0); - // _delay(200); - // PhaseCurrent_s c = getPhaseCurrents(); - // // read the current 100 times ( arbitrary number ) - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.a = c.a*0.6f + 0.4f*c1.a; - // c.b = c.b*0.6f + 0.4f*c1.b; - // c.c = c.c*0.6f + 0.4f*c1.c; - // _delay(3); - // } - // driver->setPwm(0, 0, 0); - // // align phase A - // float ab_ratio = fabs(c.a / c.b); - // float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - // if( ab_ratio > 1.5f ){ // should be ~2 - // gain_a *= _sign(c.a); - // }else if( ab_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and B - // int tmp_pinA = pinA; - // pinA = pinB; - // pinB = tmp_pinA; - // gain_a *= _sign(c.b); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinA = pinA; - // pinA = pinC; - // pinC= tmp_pinA; - // gain_a *= _sign(c.c); - // exit_flag = 2;// signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // set phase B active and phases A and C down - // driver->setPwm(0, voltage, 0); - // _delay(200); - // c = getPhaseCurrents(); - // // read the current 50 times - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.a = c.a*0.6f + 0.4f*c1.a; - // c.b = c.b*0.6f + 0.4f*c1.b; - // c.c = c.c*0.6f + 0.4f*c1.c; - // _delay(3); - // } - // driver->setPwm(0, 0, 0); - // float ba_ratio = fabs(c.b/c.a); - // float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - // if( ba_ratio > 1.5f ){ // should be ~2 - // gain_b *= _sign(c.b); - // }else if( ba_ratio < 0.7f ){ // it should be ~0.5 - // // switch phase A and B - // int tmp_pinB = pinB; - // pinB = pinA; - // pinA = tmp_pinB; - // gain_b *= _sign(c.a); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinB = pinB; - // pinB = pinC; - // pinC = tmp_pinB; - // gain_b *= _sign(c.c); - // exit_flag = 2; // signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // if phase C measured - // if(_isset(pinC)){ - // // set phase B active and phases A and C down - // driver->setPwm(0, 0, voltage); - // _delay(200); - // c = getPhaseCurrents(); - // // read the adc voltage 500 times ( arbitrary number ) - // for (int i = 0; i < 50; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.c = (c.c+c1.c)/50.0f; - // } - // driver->setPwm(0, 0, 0); - // gain_c *= _sign(c.c); - // } - - // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted return exit_flag; } diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index c3db74ef..35c97765 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -44,8 +44,14 @@ int InlineCurrentSense::init(){ params = _configureADCInline(drv_params,pinA,pinB,pinC); // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -80,174 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (driver==nullptr) { - SIMPLEFOC_DEBUG("CUR: No driver linked!"); - return 0; - } - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 5fdca7d7..94be0f75 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -6,10 +6,13 @@ #include "../common/time_utils.h" #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" + class InlineCurrentSense: public CurrentSense{ public: /** @@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) - private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..3aeb7350 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -48,8 +48,14 @@ int LowsideCurrentSense::init(){ if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver _driverSyncLowSide(driver->params, params); + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -86,169 +92,3 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 1652b330..6651bcd2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -7,6 +7,8 @@ #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,30 +36,9 @@ class LowsideCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index 1942f60b..75ee478c 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,10 +38,9 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number - bool enable_active_high = true; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -50,7 +49,8 @@ class BLDCDriver3PWM: public BLDCDriver void setPwm(float Ua, float Ub, float Uc) override; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for all phases! * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index e8643cc5..7ba7631c 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,7 +37,6 @@ class BLDCDriver6PWM: public BLDCDriver int pwmB_h,pwmB_l; //!< phase B pwm pin number int pwmC_h,pwmC_l; //!< phase C pwm pin number int enable_pin; //!< enable pin number - bool enable_active_high = true; float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e8ccc6c6 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -44,8 +44,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -56,8 +56,8 @@ void StepperDriver2PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -84,6 +84,14 @@ int StepperDriver2PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} // Set voltage to the pwm pin void StepperDriver2PWM::setPwm(float Ua, float Ub) { diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index b349af06..1bd00db9 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -60,6 +60,15 @@ class StepperDriver2PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + /** + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..52f1c1d1 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -21,8 +21,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -33,8 +33,8 @@ void StepperDriver4PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -59,6 +59,15 @@ int StepperDriver4PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} + // Set voltage to the pwm pin void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { diff --git a/src/drivers/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h index e4b2ee42..33e7d0cf 100644 --- a/src/drivers/StepperDriver4PWM.h +++ b/src/drivers/StepperDriver4PWM.h @@ -47,6 +47,16 @@ class StepperDriver4PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + + /** + * Set phase voltages to the hardware. + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; From c05289aa01d9682031224196844c9243bb4a4bda Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:02:23 +0200 Subject: [PATCH 169/220] updated readme --- README.md | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 86551f79..9fb24dd1 100644 --- a/README.md +++ b/README.md @@ -28,18 +28,8 @@ Therefore this is an attempt to: - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 -> - Teensy4 -> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) -> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) -> - stm32 -> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) -> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378) -> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394) -> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) -> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) -> - And much more: -> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 +> - Current sensing support for Stepper motors (lowside and inline) ## Arduino *SimpleFOClibrary* v2.3.3 From 602c668fa0ddf0c011b4828bddd28b9b7503beb9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 5 Jun 2024 13:19:07 +0200 Subject: [PATCH 170/220] move setting target to start of move() #404 --- src/BLDCMotor.cpp | 5 +++-- src/StepperMotor.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fe14feff..acc5b0fe 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -393,6 +393,9 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target)) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -410,8 +413,6 @@ void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5d519f29..19c96cd7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -283,6 +283,9 @@ void StepperMotor::loopFOC() { // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target) ) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -301,8 +304,6 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; From f1b7452ad557b13ae2f44d343fff89a2534ff98e Mon Sep 17 00:00:00 2001 From: ABA Date: Sat, 11 May 2024 23:56:58 +0200 Subject: [PATCH 171/220] add MT6701 I2C sensor configuration (cherry picked from commit b25ef8e59be1caa80c40da203d7967c8f4e91ee6) --- src/sensors/MagneticSensorI2C.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 7b4ea77e..2b3db0c2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -16,6 +16,14 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .data_start_bit = 15 }; +/** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s MT6701_I2C = { + .chip_address = 0x06, + .bit_resolution = 14, + .angle_register = 0x03, + .data_start_bit = 15 +}; + // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address @@ -34,6 +42,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB + // MT6701 uses 0..5 LSB and 9..15 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -111,6 +120,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB + // MT6701 uses 0..5 LSB and 6..13 MSB readValue = ( readArray[1] & lsb_mask ); readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); return readValue; From 1c732ebf424ec95d92771ead04922477173e6649 Mon Sep 17 00:00:00 2001 From: Rob Deutsch Date: Fri, 7 Jun 2024 12:28:45 +1000 Subject: [PATCH 172/220] Added gain documentation to B_G431B_ESC1.ino --- .../hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index ab8e3bba..8d751742 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -7,6 +7,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL); +// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21 LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT); @@ -107,4 +108,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} From 93bcfe00fb0a822a934405aa35ca771f4ccb2d3d Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 15:35:42 +0200 Subject: [PATCH 173/220] use current sensing only if mpcpwm used and force LEDC for now --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../esp32/esp32_ledc_mcu.cpp | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 2 +- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/hardware_api.h | 1 + .../esp32/esp32_ledc_mcu.cpp | 109 +++++++++--------- 7 files changed, 63 insertions(+), 57 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 57ffdfb5..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 869c4dde..f76c003e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp index f2d65f8e..3487dbd7 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,7 +1,7 @@ #include "../../hardware_api.h" #include "../../../drivers/hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && (!defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC)) #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 2057463c..8b09ca4e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a2f58ac3..a212d57b 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..7df00bf1 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,6 +27,7 @@ +#define SIMPLEFOC_ESP32_USELEDC // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index a454c052..f54e0e93 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,5 +1,18 @@ #include "../../hardware_api.h" +/* +For the moment the LEDC driver implements the simplest possible way to set the PWM on esp32 while enabling to set the frequency and resolution. + +The pwm is not center aligned and moreover there are no guarantees on the proper alignement between the PWM signals. +Therefore this driver is not recommended for boards that have MCPWM. + +There are however ways to improve the LEDC driver, by not using directly espressif sdk: +https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html#ledc-api-change-pwm-signal +- We could potentially use the ledc_set_duty_with_hpoint function to set the duty cycle and the high time point to make the signals center-aligned +- We could force the use of the same ledc timer within one driver to ensure the alignement between the signals + +*/ + #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) #pragma message("") @@ -11,14 +24,10 @@ #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution #define _PWM_RES_BIT 10 // 10 bir resolution -#define _PWM_RES 1023 // 2^10-1 = 1023-1 +#define _PWM_RES 1023 // 2^10-1 = 1024-1 -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// figure out how many ledc channels are avaible +// figure out how many ledc channels are available // esp32 - 2x8=16 // esp32s2 - 8 // esp32c3 - 6 @@ -30,18 +39,16 @@ #endif -// current channel stack index +// currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channel_index = 0; - - +int channels_used = 0; typedef struct ESP32LEDCDriverParams { - int channels[6]; + int pins[6]; long pwm_frequency; } ESP32LEDCDriverParams; @@ -50,9 +57,11 @@ typedef struct ESP32LEDCDriverParams { // configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); +bool _setHighFrequency(const long freq, const int pin){ + // sets up the pwm resolution and frequency on this pin + // https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html + // from v5.x no more need to deal with channels + return ledcAttach(pin, freq, _PWM_RES_BIT); } @@ -65,13 +74,14 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used++; - int ch1 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); + // setup the channel + if (!_setHighFrequency(pwm_frequency, pinA)) return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1 }, + .pins = { pinA }, .pwm_frequency = pwm_frequency }; return params; @@ -89,15 +99,15 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 2; - int ch1 = channel_index++; - int ch2 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2 }, + .pins = { pinA, pinB }, .pwm_frequency = pwm_frequency }; return params; @@ -110,17 +120,15 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 3; - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || !_setHighFrequency(pwm_frequency, pinC)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3 }, + .pins = { pinA, pinB, pinC }, .pwm_frequency = pwm_frequency }; return params; @@ -133,19 +141,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 4; - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - int ch4 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); - _setHighFrequency(pwm_frequency, pinD, ch4); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || + !_setHighFrequency(pwm_frequency, pinC)|| !_setHighFrequency(pwm_frequency, pinD)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3, ch4 }, + .pins = { pinA, pinB, pinC, pinD }, .pwm_frequency = pwm_frequency }; return params; @@ -155,31 +160,31 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); } void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); } void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); } void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); } #endif \ No newline at end of file From 8e8ffb25102d409771a93b13706db227cc7d0098 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 18:35:40 +0200 Subject: [PATCH 174/220] adde the center-aligend ledc driver - enables 6pwm --- .../esp32/esp32_driver_mcpwm.h | 16 +- .../esp32/esp32_ledc_mcu.cpp | 242 +++++++++++++----- 2 files changed, 186 insertions(+), 72 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 10497876..26db540d 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -40,7 +40,7 @@ typedef struct { int pinA; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; + mcpwm_operator_t mcpwm_operator; mcpwm_io_signals_t mcpwm_a; mcpwm_io_signals_t mcpwm_b; mcpwm_io_signals_t mcpwm_c; @@ -50,8 +50,8 @@ 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_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; @@ -62,7 +62,7 @@ typedef struct { int pin1pwm; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; + mcpwm_operator_t mcpwm_operator; mcpwm_io_signals_t mcpwm_a; mcpwm_io_signals_t mcpwm_b; } stepper_2pwm_motor_slots_t; @@ -71,8 +71,8 @@ typedef struct { int pinAH; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; mcpwm_io_signals_t mcpwm_ah; mcpwm_io_signals_t mcpwm_bh; mcpwm_io_signals_t mcpwm_ch; @@ -86,8 +86,8 @@ typedef struct ESP32MCPWMDriverParams { long pwm_frequency; mcpwm_dev_t* mcpwm_dev; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; float deadtime; } ESP32MCPWMDriverParams; diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index f54e0e93..974db6ca 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,18 +1,5 @@ #include "../../hardware_api.h" -/* -For the moment the LEDC driver implements the simplest possible way to set the PWM on esp32 while enabling to set the frequency and resolution. - -The pwm is not center aligned and moreover there are no guarantees on the proper alignement between the PWM signals. -Therefore this driver is not recommended for boards that have MCPWM. - -There are however ways to improve the LEDC driver, by not using directly espressif sdk: -https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html#ledc-api-change-pwm-signal -- We could potentially use the ledc_set_duty_with_hpoint function to set the duty cycle and the high time point to make the signals center-aligned -- We could force the use of the same ledc timer within one driver to ensure the alignement between the signals - -*/ - #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) #pragma message("") @@ -38,52 +25,124 @@ There are however ways to improve the LEDC driver, by not using directly espress #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) + // currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channels_used = 0; +// channels from 0-7 are in group 0 and 8-15 in group 1 +// - only esp32 as of mid 2024 has the second group, all the s versions don't +int group_channels_used[2] = {0}; typedef struct ESP32LEDCDriverParams { - int pins[6]; + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; long pwm_frequency; + float dead_zone; } ESP32LEDCDriverParams; - - - -// configure High PWM frequency -bool _setHighFrequency(const long freq, const int pin){ - // sets up the pwm resolution and frequency on this pin - // https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html - // from v5.x no more need to deal with channels - return ledcAttach(pin, freq, _PWM_RES_BIT); +/* + Function to attach a channel to a pin with advanced settings + - freq - pwm frequency + - resolution - pwm resolution + - channel - ledc channel + - inverted - output inverted + - group - ledc group + + This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the + PWM signals are synchronized in pairs, while the simplefoc requires a bit more flexible configuration. + This function sets also allows configuring a channel as inverted, which is not possible with the ledcAttachPin function. + + Function returns true if the channel was successfully attached, false otherwise. +*/ +bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t freq, uint8_t resolution, bool inverted) { + + + ledc_channel_t channel = static_cast(_channel); + ledc_mode_t group = static_cast(_group); + + ledc_timer_bit_t res = static_cast(resolution); + ledc_timer_config_t ledc_timer; + ledc_timer.speed_mode = group; + ledc_timer.timer_num = LEDC_TIMER_0; + ledc_timer.duty_resolution = res; + ledc_timer.freq_hz = freq; + ledc_timer.clk_cfg = LEDC_AUTO_CLK; + if (ledc_timer_config(&ledc_timer) != ESP_OK) { + return false; + } + + // if active high is false invert + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 1 : 0; + if (inverted) pin_high_level = !pin_high_level; + + uint32_t duty = ledc_get_duty(group, channel); + ledc_channel_config_t ledc_channel; + ledc_channel.speed_mode = group; + ledc_channel.channel = channel; + ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.intr_type = LEDC_INTR_DISABLE; + ledc_channel.gpio_num = pin; + ledc_channel.duty = duty; + ledc_channel.hpoint = 0; + ledc_channel.flags.output_invert = pin_high_level; + if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + return false; + } + + return true; } - - - +// returns the number of the group that has enough channels available +// returns -1 if no group has enough channels +// +// NOT IMPLEMENTED BUT COULD BE USEFUL +// returns 2 if no group has enough channels but combined they do +int _findGroupWithChannelsAvailable(int no_channels){ + if (group_channels_used[0] + no_channels < LEDC_CHANNELS_GROUP0){ + return 0; + }else if (group_channels_used[1] + no_channels < LEDC_CHANNELS_GROUP1){ + return 1; + } + // else if (group_channels_used[0] + group_channels_used[1] + no_channels < LEDC_CHANNELS){ + // return 2; + // } + return -1; +} void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 1PWM"); // check if enough channels available - if ( channels_used + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used++; - - // setup the channel - if (!_setHighFrequency(pwm_frequency, pinA)) return SIMPLEFOC_DRIVER_INIT_FAILED; + int group = _findGroupWithChannelsAvailable(1); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup in group: ", (group)); + // configure the channel + group_channels_used[group] += 1; + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA }, + .channels = { static_cast(group_channels_used[group]) }, + .groups = { (ledc_mode_t)group }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup successful in group: ", (group)); return params; } @@ -98,18 +157,33 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channels_used + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 2; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB)) + // check if enough channels available + int group = _findGroupWithChannelsAvailable(2); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[2] = {pinA, pinB}; + for(int i = 0; i < 2; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup successful in group: ", (group)); return params; } @@ -119,18 +193,33 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channels_used + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 3; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || !_setHighFrequency(pwm_frequency, pinC)) + // check if enough channels available + int group = _findGroupWithChannelsAvailable(3); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB, pinC }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[3] = {pinA, pinB, pinC}; + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup successful in group: ", (group)); return params; } @@ -140,51 +229,76 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); // check if enough channels available - if ( channels_used + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 4; - - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || - !_setHighFrequency(pwm_frequency, pinC)|| !_setHighFrequency(pwm_frequency, pinD)) + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; - + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB, pinC, pinD }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[4] = {pinA, pinB, pinC, pinD}; + for(int i = 0; i < 4; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful in group: ", (group)); return params; } - +void _writeDutyCycle(float dc, void* params, int index){ + ledc_set_duty_with_hpoint(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index], _PWM_RES*dc, _PWM_RES/2.0*(1.0-dc)); + ledc_update_duty(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index]); +} void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); } void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); } void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); + _writeDutyCycle(dc_c, params, 2); } void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); +} + + +// TODO - implement 6PWM +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){ + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM will be implemented soon for LEDC driver!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; +} +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported"); } #endif \ No newline at end of file From ae4df18374eccfe1bc4d0ffeecf73df649434248 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 21:48:36 +0200 Subject: [PATCH 175/220] forgotten star --- src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 974db6ca..8a63dd43 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -293,8 +293,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo // TODO - implement 6PWM -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){ - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM will be implemented soon for LEDC driver!"); +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){ + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { From 26d111dd458a7e3a9f97258d14b1da5d5f4af0d3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jun 2024 08:55:34 +0200 Subject: [PATCH 176/220] added the 6wpm driver --- .../esp32/esp32_ledc_mcu.cpp | 161 ++++++++++++++---- 1 file changed, 127 insertions(+), 34 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 8a63dd43..b5bc8f9a 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -75,11 +75,12 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_timer.freq_hz = freq; ledc_timer.clk_cfg = LEDC_AUTO_CLK; if (ledc_timer_config(&ledc_timer) != ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure the timer:", LEDC_TIMER_0); return false; } // if active high is false invert - int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 1 : 0; + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0 : 1; if (inverted) pin_high_level = !pin_high_level; uint32_t duty = ledc_get_duty(group, channel); @@ -91,28 +92,31 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_channel.gpio_num = pin; ledc_channel.duty = duty; ledc_channel.hpoint = 0; - ledc_channel.flags.output_invert = pin_high_level; + ledc_channel.flags.output_invert = pin_high_level; // 0 is active high, 1 is active low if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", _channel); return false; } return true; } + +// returns the number of available channels in the group +int _availableGroupChannels(int group){ + if(group == 0) return LEDC_CHANNELS_GROUP0 - group_channels_used[0]; + else if(group == 1) return LEDC_CHANNELS_GROUP1 - group_channels_used[1]; + return 0; +} + // returns the number of the group that has enough channels available // returns -1 if no group has enough channels // // NOT IMPLEMENTED BUT COULD BE USEFUL // returns 2 if no group has enough channels but combined they do int _findGroupWithChannelsAvailable(int no_channels){ - if (group_channels_used[0] + no_channels < LEDC_CHANNELS_GROUP0){ - return 0; - }else if (group_channels_used[1] + no_channels < LEDC_CHANNELS_GROUP1){ - return 1; - } - // else if (group_channels_used[0] + group_channels_used[1] + no_channels < LEDC_CHANNELS){ - // return 2; - // } + if(no_channels <= _availableGroupChannels(0)) return 0; + if(no_channels <= _availableGroupChannels(1)) return 1; return -1; } @@ -132,11 +136,12 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // configure the channel group_channels_used[group] += 1; - if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)) { - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { .channels = { static_cast(group_channels_used[group]) }, .groups = { (ledc_mode_t)group }, @@ -177,9 +182,10 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { for(int i = 0; i < 2; i++){ group_channels_used[group]++; if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; } @@ -213,9 +219,10 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in for(int i = 0; i < 3; i++){ group_channels_used[group]++; if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; } @@ -229,31 +236,49 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); - // check if enough channels available - int group = _findGroupWithChannelsAvailable(4); - if (group < 0){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { .channels = { static_cast(0)}, .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + // not enough channels available on any individual group + // check if their combined number is enough (two channels per group) + if(_availableGroupChannels(0) >=2 && _availableGroupChannels(1) >=2){ + group = 2; + SIMPLEFOC_DEBUG("EP32-DRV: WARNING: Not enough available ledc channels for 4pwm in a single group! Using two groups!"); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in groups: 0 and 1!"); + params->groups[2] = (ledc_mode_t)1; + params->groups[3] = (ledc_mode_t)1; + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough available ledc channels for 4pwm!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + params->groups[0] = (ledc_mode_t)group; + params->groups[1] = (ledc_mode_t)group; + params->groups[2] = (ledc_mode_t)group; + params->groups[3] = (ledc_mode_t)group; + } + + + int pins[4] = {pinA, pinB, pinC, pinD}; for(int i = 0; i < 4; i++){ - group_channels_used[group]++; - if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + group_channels_used[params->groups[i]]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[params->groups[i]], params->groups[i], pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - params->channels[i] = static_cast(group_channels_used[group]); - params->groups[i] = (ledc_mode_t)group; + params->channels[i] = static_cast(group_channels_used[params->groups[i]]); } - SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful in group: ", (group)); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful!"); return params; } @@ -292,13 +317,81 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo } -// TODO - implement 6PWM 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){ - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported!"); - return SIMPLEFOC_DRIVER_INIT_FAILED; + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 6PWM"); + SIMPLEFOC_DEBUG("EP32-DRV: WARNING - 6PWM on LEDC is poorly supported and not tested, consider using MCPWM driver instead!"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(6); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)group }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + int high_side_invert = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? false : true; + int low_side_invert = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? true : false; + + int pin_pairs[6][2] = { + {pinA_h, pinA_l}, + {pinB_h, pinB_l}, + {pinC_h, pinC_l} + }; + + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][0], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, high_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i] = static_cast(group_channels_used[group]); + params->groups[2*i] = (ledc_mode_t)group; + + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][1], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, low_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i+1] = static_cast(group_channels_used[group]); + params->groups[2*i+1] = (ledc_mode_t)group; + } + + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup successful in group: ", (group)); + return params; } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported"); + +void _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ + float pwm_h = _constrain(val - dead_time/2.0, 0, 1.0); + float pwm_l = _constrain(val + dead_time/2.0, 0, 1.0); + + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + _writeDutyCycle(0, params, ind_h); + }else{ + _writeDutyCycle(pwm_h, params, ind_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + _writeDutyCycle(0, params, ind_l); + }else{ + _writeDutyCycle(pwm_l, params, ind_l); + } +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPairDutyCycle(params, 0, 1, dc_a, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[0]); + _setPwmPairDutyCycle(params, 2, 3, dc_b, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[1]); + _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); } #endif \ No newline at end of file From ee91e27eb79413ee4ffcec930a41ce01c0e2ad33 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jun 2024 09:33:33 +0200 Subject: [PATCH 177/220] removed the forced ledc --- src/drivers/hardware_api.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 7df00bf1..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -25,10 +25,6 @@ #define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif - - -#define SIMPLEFOC_ESP32_USELEDC - // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) From 53624e48db35b5bae85c9d68c25ab5e41efd7b3d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 10 Jun 2024 17:04:45 +0200 Subject: [PATCH 178/220] added the inital support for new mcpwm driver --- src/communication/SimpleFOCDebug.cpp | 15 + src/communication/SimpleFOCDebug.h | 4 +- .../esp32/esp32_driver_mcpwm.cpp | 447 ++++++++++++++ .../esp32/esp32_driver_mcpwm.h | 205 ++++--- .../esp32/esp32_ledc_mcu.cpp | 7 + .../hardware_specific/esp32/esp32_mcu.cpp | 534 ++++++----------- .../teensy/teensy4_mcu1.cpp.new | 543 ------------------ 7 files changed, 762 insertions(+), 993 deletions(-) create mode 100644 src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp delete mode 100644 src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index 3bb62bce..4d50b87c 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -38,6 +38,7 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str) { } } + void SimpleFOCDebug::println(const char* str, float val) { if (_debugPrint != NULL) { _debugPrint->print(str); @@ -86,6 +87,20 @@ void SimpleFOCDebug::print(const __FlashStringHelper* str) { } } +void SimpleFOCDebug::print(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->print(str.c_str()); + } +} + + +void SimpleFOCDebug::println(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->println(str.c_str()); + } +} + + void SimpleFOCDebug::print(int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 4fcfd538..668e08af 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -33,13 +33,14 @@ **/ -#ifndef SIMPLEFOC_DISABLE_DEBUG +#ifndef SIMPLEFOC_DISABLE_DEBUG class SimpleFOCDebug { public: static void enable(Print* debugPrint = &Serial); static void println(const __FlashStringHelper* msg); + static void println(const StringSumHelper msg); static void println(const char* msg); static void println(const __FlashStringHelper* msg, float val); static void println(const char* msg, float val); @@ -52,6 +53,7 @@ class SimpleFOCDebug { static void print(const char* msg); static void print(const __FlashStringHelper* msg); + static void print(const StringSumHelper msg); static void print(int val); static void print(float val); diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp new file mode 100644 index 00000000..87f78788 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -0,0 +1,447 @@ +/* + * MCPWM in Espressif v5.x has + * - 2x groups (units) + * each one has + * - 3 timers + * - 3 operators (that can be associated with any timer) + * which control a 2xPWM signals + * - 1x comparator + 1x generator per PWM signal for independent mode + * - 1x comparator + 2x generator per pair or PWM signals for complementary mode + * + * Independent mode: + * ------------------ + * 6 PWM independent signals per unit + * unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) + * + * group | timer | operator | comparator | generator | pwm + * -------------------------------------------------------------------------------- + * 0-1 | 0-2 | 0 | 0 | 0 | A + * 0-1 | 0-2 | 0 | 1(0 complementary) | 1 | B + * 0-1 | 0-2 | 1 | 0 | 0 | A + * 0-1 | 0-2 | 1 | 1(0 complementary) | 1 | B + * 0-1 | 0-2 | 2 | 0 | 0 | A + * 0-1 | 0-2 | 2 | 1(0 complementary) | 1 | B + * + * Complementary mode + * ------------------ + * - : 3 pairs of complementary PWM signals per unit + * unit(0/1) > timer(0) > operator(0-2) > comparator(0) > generator(0-1) > pwm(A-B pair) + * + * group | timer | operator | comparator | generator | pwm + * ------------------------------------------------------------------------ + * 0-1 | 0 | 0 | 0 | 0 | A + * 0-1 | 0 | 0 | 0 | 1 | B + * 0-1 | 0 | 1 | 0 | 0 | A + * 0-1 | 0 | 1 | 0 | 1 | B + * 0-1 | 0 | 2 | 0 | 0 | A + * 0-1 | 0 | 2 | 0 | 1 | B + * + * More info + * ---------- + * - timers can be associated with any operator, and multiple operators can be associated with the same timer + * - comparators can be associated with any operator + * - two comparators per operator for independent mode + * - one comparator per operator for complementary mode + * - generators can be associated with any comparator + * - one generator per PWM signal for independent mode + * - two generators per pair of PWM signals for complementary mode + * - dead-time can be set for each generator pair in complementary mode + * + * Docs + * ------- + * More info here: https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm + * and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +*/ +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "esp32_driver_mcpwm.h" + +// MCPWM driver hardware timer pointers +mcpwm_timer_handle_t timers[2][3] = {NULL}; +// MCPWM timer periods configured (directly related to the pwm frequency) +uint32_t pwm_periods[2][3]; +// how many pins from the groups 6 pins is used +uint8_t group_pins_used[2] = {0}; +// last operator in the group +mcpwm_oper_handle_t last_operator[2]; + + + +// checking if group has pins available +bool _hasAvailablePins(int group, int no_pins){ + if(group_pins_used[group] + no_pins > 6){ + return false; + } + return true; +} + +// returns the index of the last timer in the group +// -1 if no timer instantiated yet +uint8_t _findLastTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i-1; + } + } + // return the last index + return i; +} +// returns the index of the next timer to instantiate +// -1 if no timers available +uint8_t _findNextTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i; + } + } + return -1; +} + +/* + * find the best group for the pins + * if 6pwm + * - Only option is an an empty group + * if 3pwm + * - Best is an empty group (we can set a pwm frequency) + * - Second best is a group with 4pwms (2 operators) available (we can set the pwm frequency -> new timer+new operator) + * - Third best option is any group which has 3pwms available (but uses previously defined pwm_frequency) + * if 1pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms (one operator) available (we can set the pwm frequency -> new timer+new operator) + * - Third best is a group with 1pwm available (but uses previously defined pwm_frequency ) + * if 2pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms available (we can set the pwm frequency -> new timer+new operator) + * - Third best is one pin per group (but uses previously defined pwm_frequency ) + * if 4pwm + * - best option is an empty group (we can set the pwm frequency) + * - second best is a group with 4pwms available (we can set the pwm frequency -> new timer + new operators) + * - third best is 2pwms per group (we can set the pwm frequency -> new timers + new operators) + * + * PROBLEM: Skipping/loosing channels happens in some cases when the group has already used some odd number of pwm channels (for example 3pwm or 1pwm) + * For example if the group has already used 3pwms, there is one generator that has one pwm channel left. + * If we use this channel we have to use the same timer it has been used with before, so we cannot change the pwm frequency. + * Current implementation does use the remaining channel only if there isn't other options that would allow changing the pwm frequency. + * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel + * and use a new timer and operator to allow changing the pwm frequency. In such cases we loose (cannot be used) the remaining channel. + * TODO: use the pwm_frequency to avoid skipping pwm channels ! + * + * returns + * - 1 if solution found in one group + * - 2 if solution requires using both groups + * - 0 if no solution possible +*/ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ + // an empty group is always the best option + for(int i=0; i<2; i++){ + if(!group_pins_used[i]){ + *group = i; + *timer=0; // use the first timer in an empty group + return 1; + } + } + + // if 3 or 1pwm + // check if there is available space in one of the groups + // otherwise fail + if(no_pins == 3 || no_pins==1){ + // second best option is if there is a group with + // pair number of pwms available as we can then + // set the pwm frequency + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins+1)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findLastTimer(i); + return 1; + } + } + } + + // if 2 or 4 pwm + // check if there is available space in one of the groups + // if not check if they can be separated in two groups + if(no_pins == 2 || no_pins==4){ + // second best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is half pwms per group + int half_no_pins = (int)no_pins/2; + if(_hasAvailablePins(0,half_no_pins) && _hasAvailablePins(1 ,half_no_pins)){ + return 2; + } + } + + // otherwise fail + return 0; +} + + +// configuring center aligned pwm +// More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low +void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ + if(inverted) + mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); + else + mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); +} + + + +// Helper function calculating the pwm period from the pwm frequency +// - pwm_frequency - pwm frequency in hertz +// returns pwm period in ticks (uint32_t) +uint32_t _calcPWMPeriod(long pwm_frequency) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_frequency); +} +/* + Helper function calculating the pwm frequency from the pwm period + - pwm_period - pwm period in ticks + returns pwm frequency in hertz (long) +*/ +long _calcPWMFreq(long pwm_period) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_period / 2); +} + +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins){ + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + + uint8_t no_operators = 3; // use 3 comparators one per pair of pwms + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with hardware dead-time"); + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + int no_generators = 6; // one per pwm + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_generators) + " generators."); + // Create and configure generators + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_generators; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring Center-Aligned 6 pwm."); + for (int i = 0; i < 3; i++) { + _configureCenterAlign(params->generator[2*i],params->comparator[i]); + } + // only available for 6pwm + SIMPLEFOC_ESP32_DEBUG("Configuring dead-time."); + uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; + mcpwm_dead_time_config_t dt_config_high; + dt_config_high.posedge_delay_ticks = dead_time; + dt_config_high.negedge_delay_ticks = 0; + dt_config_high.flags.invert_output = !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH; + mcpwm_dead_time_config_t dt_config_low; + dt_config_low.posedge_delay_ticks = 0; + dt_config_low.negedge_delay_ticks = dead_time; + dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; + for (int i = 0; i < 3; i++) { + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + } +#else // software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with software dead-time"); + int no_pins = 6; + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < 3; i++) { + _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } +#endif + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + params->dead_zone = dead_zone; + // save the configuration variables for later + group_pins_used[mcpwm_group] = 6; + return params; +} + + +/* + function configuring the pins for the mcpwm + - pwm_frequency - pwm frequency + - mcpwm_group - mcpwm group + - timer_no - timer number + - no_pins - number of pins + - pins - array of pins + - dead_zone - dead zone + + returns the driver parameters +*/ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins){ + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + bool shared_timer = false; + // check if timer is configured + if (timers[mcpwm_group][timer_no] == NULL){ + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + // initialise the timer + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + // save variables for later + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + // if the numer of used channels it not pair skip one channel + // the skipped channel cannot be used with the new timer + // TODO avoid loosing channels like this + if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; + }else{ + // we will use an already instantiated timer + params->timers[0] = timers[mcpwm_group][timer_no]; + SIMPLEFOC_ESP32_DEBUG("Using previously configured timer: " + String(timer_no)); + // but we cannot change its configuration without affecting the other drivers + // so let's first verify that the configuration is the same + if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ + SIMPLEFOC_ESP32_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); + + shared_timer = true; + } + + uint8_t no_operators = ceil(no_pins / 2.0); + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + for (int i = 0; i < no_operators; i++) { + if (shared_timer && i == 0) { // first operator already configured + params->oper[0] = last_operator[mcpwm_group]; + continue; + } + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + // save the last operator in this group + last_operator[mcpwm_group] = params->oper[no_operators - 1]; + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + + SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < no_pins; i++) { + _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); + } + + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + // save the configuration variables for later + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + group_pins_used[mcpwm_group] += no_pins; + return params; +} + +// function setting the duty cycle to the MCPWM pin +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 26db540d..d0b7933e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,92 +5,143 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" +#include "esp_idf_version.h" +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif -#pragma message("") -#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") -#pragma message("") +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif +//!< ESP32 MCPWM driver parameters +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; //!< frequency of the pwm signal + int group_id; //!< group of the mcpwm + mcpwm_timer_handle_t timers[2]; //!< timers of the mcpwm + mcpwm_oper_handle_t oper[3]; //!< operators of the mcpwm + mcpwm_cmpr_handle_t comparator[6]; //!< comparators of the mcpwm + mcpwm_gen_handle_t generator[6]; //!< generators of the mcpwm + uint32_t mcpwm_period; //!< period of the pwm signal + float dead_zone; //!< dead zone of the pwm signal +} ESP32MCPWMDriverParams; -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 +#define SIMPLEFOC_ESP32_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-DRV: " + String(str));\ + +// macro for checking the error of the mcpwm functions +// if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED +#define CHECK_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_DRIVER_INIT_FAILED; \ + } + // ABI bus frequency - would be better to take it from somewhere // but I did nto find a good exposed variable #define _MCPWM_FREQ 160e6f - -// preferred pwm resolution default -#define _PWM_RES_DEF 4096 -// min resolution -#define _PWM_RES_MIN 3000 -// max resolution -#define _PWM_RES_MAX 8000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_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_4pwm_motor_slots_t; - -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - 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_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; - - -typedef struct ESP32MCPWMDriverParams { - long pwm_frequency; - mcpwm_dev_t* mcpwm_dev; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - float deadtime; -} ESP32MCPWMDriverParams; - +#define _PWM_TIMEBASE_RESOLUTION_HZ (_MCPWM_FREQ) /*!< Resolution of MCPWM */ +// pwm frequency settings +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50kHz + + +// low-level configuration API + +/** + * checking if group has pins available + * @param group - group of the mcpwm + * @param no_pins - number of pins + * @returns true if pins are available, false otherwise + */ +bool _hasAvailablePins(int group, int no_pins); +/** + * function finding the last timer in the group + * @param group - group of the mcpwm + * @returns index of the last timer in the group + * -1 if no timer instantiated yet + */ +uint8_t _findLastTimer(int group); + +/** + * function finding the next timer in the group + * @param group - group of the mcpwm + * @returns index of the next timer in the group + * -1 if all timers are used + */ +uint8_t _findNextTimer(int group); + + +/** + * function finding the best group and timer for the pwm signals + * + * @param no_pins - number of pins + * @param pwm_freq - frequency of the pwm signal + * @param group - pointer to the group + * @param timer - pointer to the timer + * @returns + * 1 if solution found in one group + * 2 if solution requires using both groups + * 0 if no solution possible + */ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); + + +/** + * function configuring the center alignement and inversion of a pwm signal + * @param gena - mcpwm generator handle + * @param cmpa - mcpwm comparator handle + * @param inverted - true if the signal is inverted, false otherwise + */ +void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); + +/** + * function calculating the pwm period + * @param pwm_frequency - frequency of the pwm signal + * @return uint32_t - period of the pwm signal + */ +uint32_t _calcPWMPeriod(long pwm_frequency); +/** + * function calculating the pwm frequency + * @param pwm_period - period of the pwm signal + * @return long - frequency of the pwm signal + */ +long _calcPWMFreq(long pwm_period); + +/** + * function configuring the MCPWM for 6pwm + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param dead_zone - dead zone of the pwm signal + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins); +/** + * function configuring the MCPWM for pwm generation + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param no_pins - number of pins + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins); +/** + * function setting the duty cycle to the MCPWM channel + * @param cmpr - mcpwm channel handle + * @param mcpwm_period - period of the pwm signal + * @param duty_cycle - duty cycle of the pwm signal + */ +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index b5bc8f9a..3c413725 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -7,6 +7,13 @@ #pragma message("") #include "driver/ledc.h" +#include "esp_idf_version.h" + + +// version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 0dc23c10..33f9db20 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -1,431 +1,221 @@ #include "esp32_driver_mcpwm.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") -#ifndef SIMPLEFOC_ESP32_HW_DEADTIME - #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. -#endif - -// define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_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 - }; - -// define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; - -// define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; - -// define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B - }; - - -// configuring high frequency pwm timer -// a lot of help from this post from Paul Gauld -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -// a short tutorial for v2.0.1+ -// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html -// -void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ - - mcpwm_config_t pwm_config; - pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) - pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 - 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 PWM1A & PWM1B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - - if (_isset(dead_zone)){ - // dead zone is configured - - // When using hardware deadtime, setting the phase_state parameter is not supported. - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_type_t pwm_mode; - if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); - #else // Software deadtime - for (int i = 0; i < 3; i++){ - if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside - else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver - - if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside - else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver - } - #endif +// function setting the high pwm frequency to the supplied pins +// - DC motor - 1PWM setting +// - hardware specific +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + int group, timer; + if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 1PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - _delay(100); - - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); - - // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.clk_prescale = 0; - // calculate prescaler and period - // step 1: calculate the prescaler using the default pwm resolution - // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; - // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); - // now calculate the real resolution timer period necessary (pwm resolution) - // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); - // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); - resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); - - // set prescaler - mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; - _delay(1); - //set period - mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; - _delay(1); - mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; - _delay(1); - // _delay(1); - //restart the timers - mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); - - mcpwm_sync_config_t sync_conf = { - .sync_sig = MCPWM_SELECT_TIMER0_SYNC, - .timer_val = 0, - .count_direction = MCPWM_TIMER_DIRECTION_UP - }; - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); - - // Enable sync event for all timers to be the TEZ of timer0 - mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); + SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[1] = {pinA}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); } + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_2pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; - m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; - break; - } + int group, timer; + int ret = _findBestGroup(2, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 2PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - - // disable all the slots with the same MCPWM - // disable 3pwm bldc motor which would go in the same slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + if(ret == 1){ + // configure the 2pwm on only one group + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[2] = {pinA, pinB}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM as two 1PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][1] = {{pinA}, {pinB}}; + for(int i =0; i<2; i++){ + int timer = _findLastTimer(i); //find last created timer in group i + SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DEBUG("Error configuring 1PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + }; + for(int i =0; i<2; i++){ + ret_params->timers[i] = params[i]->timers[0]; + ret_params->oper[i] = params[i]->oper[0]; + ret_params->comparator[i] = params[i]->comparator[0]; + ret_params->generator[i] = params[i]->generator[0]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; } - // 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); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; } - // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +// - hardware specific +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - bldc_3pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; - m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - // disable 2pwm steppr motor which would go in the same slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + int group, timer; + if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 3PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - // 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); - + SIMPLEFOC_ESP32_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; + int pins[3] = {pinA, pinB, pinC}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); } + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic -void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +// - hardware specific +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD){ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_4pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; - break; - } + + int group, timer; + int ret = _findBestGroup(4, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 4PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slots 0 and 1 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + if(ret == 1){ + SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[4] = {pinA, pinB, pinC, pinD}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slots 2 and 3 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM as two 2PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; + for(int i =0; i<2; i++){ + int timer = _findNextTimer(i); //find next available timer in group i + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DEBUG("Error configuring 2PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + .timers = {params[0]->timers[0], params[1]->timers[0]}, + .oper = {params[0]->oper[0], params[1]->oper[0]} + }; + for(int i =0; i<4; i++){ + ret_params->comparator[i] = params[(int)floor(i/2)]->comparator[i%2]; + ret_params->generator[i] = params[(int)floor(i/2)]->generator[i%2]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; } - - // 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); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2 - }; - return params; } +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){ + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); + int group, timer; + if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 6PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; + return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); } - - - // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic -// ESP32 uses MCPWM void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); } - - - // function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); +// - DCMotor -1PWM setting +// - hardware specific +void _writeDutyCycle1PWM(float dc_a, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); } - - - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting // - hardware specific -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){ - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency - bldc_6pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - break; - } - } - // if no slots available - if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2, - .deadtime = _isset(dead_zone) ? dead_zone : 0 - }; - return params; +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); } - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // set the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - // Hardware deadtime does deadtime insertion internally - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); - _UNUSED(phase_state); - #else - float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - #endif +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2b); } +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +#if SIMPLEFOC_ESP32_HW_DEADTIME == true + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); +#else + uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; + float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? dc_a-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? dc_a+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? dc_b-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? dc_b+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[4], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? dc_c-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); +#endif +} #endif diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new deleted file mode 100644 index 5dcac90d..00000000 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new +++ /dev/null @@ -1,543 +0,0 @@ -#include "teensy_mcu.h" -#include "teensy4_mcu.h" -#include "../../../communication/SimpleFOCDebug.h" - -// if defined -// - Teensy 4.0 -// - Teensy 4.1 -#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - - -#pragma message("") -#pragma message("SimpleFOC: compiling for Teensy 4.x") -#pragma message("") - - - -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} - -// function finding the EXT_SYNC event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 -int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - } - return -1; -} - -// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper -// function to make code more readable. -void xbar_connect(unsigned int input, unsigned int output) -{ - if (input >= 88) return; - if (output >= 132) return; - volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); - uint16_t val = *xbar; - if (!(output & 1)) { - val = (val & 0xFF00) | input; - } else { - val = (val & 0x00FF) | (input << 8); - } - *xbar = val; -} - -void xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} - -// half_cycle of the PWM variable -int half_cycle = 0; - -// function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm; - switch ((info->module >> 4) & 3) { - case 0: flexpwm = &IMXRT_FLEXPWM1; break; - case 1: flexpwm = &IMXRT_FLEXPWM2; break; - case 2: flexpwm = &IMXRT_FLEXPWM3; break; - default: flexpwm = &IMXRT_FLEXPWM4; - } - if(flexpwm != nullptr){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); -#endif - return flexpwm; - } - return nullptr; -} - - -// function which finds the timer submodule for a pin -// if it does not belong to the submodule it returns a -1 -int get_submodule(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - - if (sm1 >= 0 && sm1 < 4) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); - SIMPLEFOC_DEBUG(s); -#endif - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - -// function which finds the flexpwm instance for a pair of pins -// if they do not belong to the same timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; - switch ((info->module >> 4) & 3) { - case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; - default: flexpwm1 = &IMXRT_FLEXPWM4; - } - - info = pwm_pin_info + pin1; - switch ((info->module >> 4) & 3) { - case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; - default: flexpwm2 = &IMXRT_FLEXPWM4; - } - if(flexpwm1 == flexpwm2){ - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); - return flexpwm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_submodule(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - info = pwm_pin_info + pin1; - int sm2 = info->module&0x3; - - if (sm1 == sm2) { - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); - SIMPLEFOC_DEBUG(s); - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_inverted_channel(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; - - if (ch1 != 1) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else if (ch2 != 2) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); - SIMPLEFOC_DEBUG(s); -#endif -return ch2; - } -} - -// Helper to set up A/B pair on a FlexPWM submodule. -// can configure sync, prescale and B inversion. -// sets the desired frequency of the PWM -// sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) -{ - int submodule_mask = 1 << submodule ; - flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running - flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK - - - // calculate the counter and prescaler for the desired pwm frequency - uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); - uint32_t prescale = 0; - //printf(" div=%lu\n", newdiv); - while (newdiv > 65535 && prescale < 7) { - newdiv = newdiv >> 1; - prescale = prescale + 1; - } - if (newdiv > 65535) { - newdiv = 65535; - } else if (newdiv < 2) { - newdiv = 2; - } - - // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); - int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% - int mid_pwm = int((half_cycle)/2.0f); - - // if the timer should be externally synced with the master timer - int sel = ext_sync ? 3 : 0; - - // setup the timer - // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h - flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); - flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; - // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; - if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer - flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0; - flexpwm->SM[submodule].VAL1 = half_cycle ; - flexpwm->SM[submodule].VAL2 = -mid_pwm ; - flexpwm->SM[submodule].VAL3 = +mid_pwm ; - - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled - flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running -} - - -// staring the PWM on A and B channels of the submodule -void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) -{ - int submodule_mask = 1 << submodule ; - - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output -} - - - -// PWM setting on the high and low pair of the PWM channels -void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); - int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - - flexpwm->SM[submodule].VAL2 = -count_pwm; // A on - flexpwm->SM[submodule].VAL3 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - int count_pwm = int(count*duty); - - SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); - SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); - SIMPLEFOC_DEBUG("count: ",count_pwm); - - // flexpwm->SM[submodule].VAL1 = 0; // A on - flexpwm->SM[submodule].VAL2 = count_pwm ; // A off - flexpwm->SM[submodule].VAL3 = count_pwm; // A on - flexpwm->SM[submodule].VAL4 = count_pwm ; // A off - flexpwm->SM[submodule].VAL5 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; - if (cval > modulo) cval = modulo; // TODO: is this check correct? - - //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", - //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); - p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); - switch (channel) { - case 0: // X - p->SM[submodule].VAL0 = modulo - cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); - //printf(" write channel X\n"); - break; - case 1: // A - p->SM[submodule].VAL3 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); - //printf(" write channel A\n"); - break; - case 2: // B - p->SM[submodule].VAL5 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); - //printf(" write channel B\n"); - } - p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 6PWM setting -// - hardware specific -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) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA_h,pinA_l); - submoduleA = get_submodule(pinA_h,pinA_l); - inverted_channelA = get_inverted_channel(pinA_h,pinA_l); - flexpwmB = get_flexpwm(pinB_h,pinB_l); - submoduleB = get_submodule(pinB_h,pinB_l); - inverted_channelB = get_inverted_channel(pinB_h,pinB_l); - flexpwmC = get_flexpwm(pinC_h,pinC_l); - submoduleC = get_submodule(pinC_h,pinC_l); - inverted_channelC = get_inverted_channel(pinC_h,pinC_l); - - if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - - - // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced - setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; - delayMicroseconds (100) ; - - // turn on XBAR1 clock for all but stop mode - xbar_init() ; - - // // Connect trigger to synchronize all timers - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - - delayMicroseconds(50) ; - // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; - *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; - *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; - *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; - *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; - *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; - - return params; -} - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// in generic case dont do anything - void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA); - submoduleA = get_submodule(pinA); - flexpwmB = get_flexpwm(pinB); - submoduleB = get_submodule(pinB); - flexpwmC = get_flexpwm(pinC); - submoduleC = get_submodule(pinC); - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA, pinB, pinC }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - }; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - // analogWriteFrequency(pinA, pwm_frequency); - // analogWriteFrequency(pinB, pwm_frequency); - // analogWriteFrequency(pinC, pwm_frequency); - // analogWrite(pinA, 0); - // analogWrite(pinB, 0); - // analogWrite(pinC, 0); - - // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; - // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; - // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; - - return params; -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -#endif \ No newline at end of file From e221e063ba90a3fee7b05b6048361f8423f28f5f Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 10 Jun 2024 17:05:15 +0200 Subject: [PATCH 179/220] disable current sensing --- src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp | 2 +- src/current_sense/hardware_specific/esp32/esp32_adc_driver.h | 2 +- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 2 +- src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..4a37ddb8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f76c003e..357b35b0 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 8b09ca4e..d334008e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a212d57b..31cbb027 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" From a7ed4ba247782915dd5ccbeb8d8258a2fb8e3951 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Jun 2024 17:49:38 +0200 Subject: [PATCH 180/220] an initial implementation of the lowside and inline current sense with the new esp32 api --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 162 +++++++----------- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/BLDCDriver3PWM.cpp | 2 + src/drivers/BLDCDriver6PWM.cpp | 2 + src/drivers/StepperDriver2PWM.cpp | 2 + src/drivers/StepperDriver4PWM.cpp | 2 + src/drivers/hardware_api.h | 11 ++ .../esp32/esp32_driver_mcpwm.cpp | 26 +-- .../esp32/esp32_driver_mcpwm.h | 8 + .../hardware_specific/esp32/esp32_mcu.cpp | 13 ++ src/drivers/hardware_specific/generic_mcu.cpp | 6 + 13 files changed, 128 insertions(+), 112 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 4a37ddb8..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 357b35b0..c79afdb4 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index d334008e..f3131a1e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,26 +2,26 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" +#include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" - #include #include #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f +// set the pin 19 in high during the adc interrupt +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG typedef struct ESP32MCPWMCurrentSenseParams { int pins[3]; float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; } ESP32MCPWMCurrentSenseParams; @@ -36,8 +36,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); + SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC inline"); if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); @@ -51,30 +51,15 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p } - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; + ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; } // not found return 0; @@ -83,83 +68,68 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ // function configuring low-side current sensing void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; + SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC low-side"); + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + if(mcpwm_timer_start_stop(p->timers[0], MCPWM_TIMER_START_NO_STOP) != ESP_ERR_INVALID_STATE){ + // if we get the invalid state error it means that the timer is not enabled + // that means that we can configure it for low-side current sensing + SIMPLEFOC_DEBUG("ESP32-CS: ERR - The timer is already enabled. Cannot be configured for low-side current sensing."); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + int no_adc_channels = 0; + if( _isset(pinA) ){ + pinMode(pinA, INPUT); + params->pins[no_adc_channels++] = pinA; + } + if( _isset(pinB) ){ + pinMode(pinB, INPUT); + params->pins[no_adc_channels++] = pinB; + } + if( _isset(pinC) ){ + pinMode(pinC, INPUT); + params->pins[no_adc_channels++] = pinC; + } + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; return params; } void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(19, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + mcpwm_timer_event_callbacks_t cbs_timer = { + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + digitalWrite(19, HIGH); + #endif + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // sample the phase currents one at a time + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + digitalWrite(19, LOW); + #endif + return true; + } + }; + if(mcpwm_timer_register_event_callbacks(p->timers[0], &cbs_timer, cs_params) != ESP_OK){ + SIMPLEFOC_DEBUG("ESP32-CS: ERR - Failed to sync ADC and driver"); + } } -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 31cbb027..11149b4a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 637c8db5..f08c9f11 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,6 +20,8 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 4981858f..ba19becf 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,6 +24,8 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set phase state enabled diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e5f1930c 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -43,6 +43,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..f584a5b9 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -20,6 +20,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 7809233d..07abb21e 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,6 +27,7 @@ // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) +#define SIMPLEFOC_DRIVER_INIT_SUCCESS ((void*)1) // generic implementation of the hardware specific structure // containing all the necessary driver parameters @@ -173,5 +174,15 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo */ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params); +/** + * Function enabling the PWM outputs + * - hardware specific + * + * @param params the driver parameters + * + * @return the pointer to the driver parameters if successful, -1 if failed + */ +void* _enablePWM(void* params); + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 87f78788..1686bc0c 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -320,10 +320,6 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); - // Enable and start timer - CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); - CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); @@ -424,12 +420,6 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); - // Enable and start timer if not shared - if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); - // start the timer - CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); - _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); // save the configuration variables for later @@ -439,9 +429,19 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } // function setting the duty cycle to the MCPWM pin -void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); - mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +bool _enableTimer(mcpwm_timer_handle_t timer){ + int ret = mcpwm_timer_enable(timer); // enable the timer + if (ret == ESP_ERR_INVALID_STATE){ // if already enabled + SIMPLEFOC_ESP32_DEBUG("Timer already enabled: "+String(i)); + }else if(ret != ESP_OK){ + SIMPLEFOC_ESP32_DEBUG("Failed to enable timer!"); // if failed + return false; + } + if(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP)!=ESP_OK){ + SIMPLEFOC_ESP32_DEBUG("Failed to start the timer!"); + return false; + } } + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index d0b7933e..4aa4f45e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -143,5 +143,13 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * Function checking if timer is enabled + * @param timer - mcpwm timer handle + * + * @returns true if timer is enabled, false otherwise + */ +bool _enableTimer(mcpwm_timer_handle_t timer); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 33f9db20..bc1990f4 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -218,4 +218,17 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); #endif } + +void* _enablePWM(void* params){ + SIMPLEFOC_ESP32_DEBUG("Enabling timers."); + ESP32MCPWMDriverParams* p = (ESP32MCPWMDriverParams*)params; + for (int i=0; i<2; i++){ + if (p->timers[i] == nullptr) continue; // if only one timer + if(!_enableTimer(p->timers[i])){ + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + } + return params; +} + #endif diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index b6bc2f06..3d648d29 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -122,4 +122,10 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(dc_c); _UNUSED(phase_state); _UNUSED(params); +} + +// function enabling the power stage +// - hardware specific +__attribute__((weak)) void* _enablePWM(void* params){ + return params; } \ No newline at end of file From ab64524dd34fbfcde295c9b3478c60ab0e1bc791 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 09:51:12 +0200 Subject: [PATCH 181/220] forgotten import --- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index f3131a1e..f4a95473 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -4,11 +4,19 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "esp32_adc_driver.h" + #include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" #include #include +#include "esp_idf_version.h" + +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f From aa8591e4ea0a2c5c744afe9285e93f0bca416a46 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 15:47:36 +0200 Subject: [PATCH 182/220] Revert "an initial implementation of the lowside and inline current sense with the new esp32 api" This reverts commit a7ed4ba247782915dd5ccbeb8d8258a2fb8e3951. --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 166 ++++++++++-------- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/BLDCDriver3PWM.cpp | 2 - src/drivers/BLDCDriver6PWM.cpp | 2 - src/drivers/StepperDriver2PWM.cpp | 2 - src/drivers/StepperDriver4PWM.cpp | 2 - src/drivers/hardware_api.h | 11 -- .../esp32/esp32_driver_mcpwm.cpp | 26 +-- .../esp32/esp32_driver_mcpwm.h | 8 - .../hardware_specific/esp32/esp32_mcu.cpp | 13 -- src/drivers/hardware_specific/generic_mcu.cpp | 6 - 13 files changed, 110 insertions(+), 134 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..4a37ddb8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index c79afdb4..357b35b0 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index f4a95473..d334008e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,34 +2,26 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "esp32_adc_driver.h" -#include "driver/mcpwm_prelude.h" +#include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" + #include #include -#include "esp_idf_version.h" - -// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 -#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) -#endif #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f -// set the pin 19 in high during the adc interrupt -// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG typedef struct ESP32MCPWMCurrentSenseParams { int pins[3]; float adc_voltage_conv; - int adc_buffer[3] = {}; - int buffer_index = 0; - int no_adc_channels = 0; + mcpwm_unit_t mcpwm_unit; + int buffer_index; } ESP32MCPWMCurrentSenseParams; @@ -44,8 +36,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + _UNUSED(driver_params); - SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC inline"); if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); @@ -59,15 +51,30 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p } + +/** + * Low side adc reading implementation +*/ + +static void IRAM_ATTR mcpwm0_isr_handler(void*); +static void IRAM_ATTR mcpwm1_isr_handler(void*); +byte currentState = 1; +// two mcpwm units +// - max 2 motors per mcpwm unit (6 adc channels) +int adc_pins[2][6]={0}; +int adc_pin_count[2]={0}; +uint32_t adc_buffer[2][6]={0}; +int adc_read_index[2]={0}; + // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; - int no_channel = 0; - for(int i=0; i < 3; i++){ - if(!_isset(p->pins[i])) continue; - if(pin == p->pins[i]) // found in the buffer - return p->adc_buffer[no_channel] * p->adc_voltage_conv; - else no_channel++; + mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; + int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; + float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; + + for(int i=0; i < adc_pin_count[unit]; i++){ + if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; } // not found return 0; @@ -76,68 +83,83 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ // function configuring low-side current sensing void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC low-side"); - // check if driver timer is already running - // fail if it is - // the easiest way that I've found to check if timer is running - // is to start it and stop it - ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; - if(mcpwm_timer_start_stop(p->timers[0], MCPWM_TIMER_START_NO_STOP) != ESP_ERR_INVALID_STATE){ - // if we get the invalid state error it means that the timer is not enabled - // that means that we can configure it for low-side current sensing - SIMPLEFOC_DEBUG("ESP32-CS: ERR - The timer is already enabled. Cannot be configured for low-side current sensing."); - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } + mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + int index_start = adc_pin_count[unit]; + if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; + if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; + if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; - int no_adc_channels = 0; - if( _isset(pinA) ){ - pinMode(pinA, INPUT); - params->pins[no_adc_channels++] = pinA; - } - if( _isset(pinB) ){ - pinMode(pinB, INPUT); - params->pins[no_adc_channels++] = pinB; - } - if( _isset(pinC) ){ - pinMode(pinC, INPUT); - params->pins[no_adc_channels++] = pinC; - } + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), + .mcpwm_unit = unit, + .buffer_index = index_start + }; - params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); - params->no_adc_channels = no_adc_channels; return params; } void _driverSyncLowSide(void* driver_params, void* cs_params){ -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - pinMode(19, OUTPUT); -#endif - ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + + mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; + mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + + // low-side register enable interrupt + mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // high side registers enable interrupt + //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt + + // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) + if(mcpwm_unit == MCPWM_UNIT_0) + mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler + else + mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm0_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - mcpwm_timer_event_callbacks_t cbs_timer = { - .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ - ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; - #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - digitalWrite(19, HIGH); - #endif - // increment buffer index - p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; - // sample the phase currents one at a time - p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); - #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - digitalWrite(19, LOW); - #endif - return true; - } - }; - if(mcpwm_timer_register_event_callbacks(p->timers[0], &cbs_timer, cs_params) != ESP_OK){ - SIMPLEFOC_DEBUG("ESP32-CS: ERR - Failed to sync ADC and driver"); - } + // low side + uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); + adc_read_index[0]++; + if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; + } + // low side + MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; } +static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm1_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; + + // low side + uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); + adc_read_index[1]++; + if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; + } + // low side + MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; +} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 11149b4a..31cbb027 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index f08c9f11..637c8db5 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,8 +20,6 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index ba19becf..4981858f 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,8 +24,6 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set phase state enabled diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index e5f1930c..dbbf5b8f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -43,8 +43,6 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f584a5b9..836f5472 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -20,8 +20,6 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 07abb21e..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,7 +27,6 @@ // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) -#define SIMPLEFOC_DRIVER_INIT_SUCCESS ((void*)1) // generic implementation of the hardware specific structure // containing all the necessary driver parameters @@ -174,15 +173,5 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo */ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params); -/** - * Function enabling the PWM outputs - * - hardware specific - * - * @param params the driver parameters - * - * @return the pointer to the driver parameters if successful, -1 if failed - */ -void* _enablePWM(void* params); - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 1686bc0c..87f78788 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -320,6 +320,10 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); @@ -420,6 +424,12 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); // save the configuration variables for later @@ -429,19 +439,9 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } // function setting the duty cycle to the MCPWM pin -bool _enableTimer(mcpwm_timer_handle_t timer){ - int ret = mcpwm_timer_enable(timer); // enable the timer - if (ret == ESP_ERR_INVALID_STATE){ // if already enabled - SIMPLEFOC_ESP32_DEBUG("Timer already enabled: "+String(i)); - }else if(ret != ESP_OK){ - SIMPLEFOC_ESP32_DEBUG("Failed to enable timer!"); // if failed - return false; - } - if(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP)!=ESP_OK){ - SIMPLEFOC_ESP32_DEBUG("Failed to start the timer!"); - return false; - } +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); } - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4aa4f45e..d0b7933e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -143,13 +143,5 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); -/** - * Function checking if timer is enabled - * @param timer - mcpwm timer handle - * - * @returns true if timer is enabled, false otherwise - */ -bool _enableTimer(mcpwm_timer_handle_t timer); - #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index bc1990f4..33f9db20 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -218,17 +218,4 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); #endif } - -void* _enablePWM(void* params){ - SIMPLEFOC_ESP32_DEBUG("Enabling timers."); - ESP32MCPWMDriverParams* p = (ESP32MCPWMDriverParams*)params; - for (int i=0; i<2; i++){ - if (p->timers[i] == nullptr) continue; // if only one timer - if(!_enableTimer(p->timers[i])){ - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - } - return params; -} - #endif diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 3d648d29..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -122,10 +122,4 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(dc_c); _UNUSED(phase_state); _UNUSED(params); -} - -// function enabling the power stage -// - hardware specific -__attribute__((weak)) void* _enablePWM(void* params){ - return params; } \ No newline at end of file From 5d4187a3eb18a7e8eb3586115dd6a3b1e3571136 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 22:54:19 +0200 Subject: [PATCH 183/220] added the current sensing + phase state for driver --- src/current_sense/LowsideCurrentSense.cpp | 3 +- src/current_sense/hardware_api.h | 5 +- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../esp32/esp32_mcpwm_mcu.cpp | 189 ++++++++++++++++++ .../hardware_specific/esp32/esp32_mcu.cpp | 165 --------------- .../esp32/esp32s_adc_driver.cpp | 2 +- .../hardware_specific/generic_mcu.cpp | 4 +- .../hardware_specific/rp2040/rp2040_mcu.cpp | 2 +- .../hardware_specific/samd/samd21_mcu.cpp | 5 +- .../stm32/b_g431/b_g431_mcu.cpp | 6 +- .../stm32/stm32f1/stm32f1_mcu.cpp | 7 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 7 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 7 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 7 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 6 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 7 +- .../esp32/esp32_driver_mcpwm.cpp | 53 +++-- .../esp32/esp32_driver_mcpwm.h | 19 +- .../esp32/esp32_ledc_mcu.cpp | 2 +- .../{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} | 37 ++-- .../hardware_specific/esp32/mcpwm_private.h | 78 ++++++++ 22 files changed, 390 insertions(+), 225 deletions(-) create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp delete mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.cpp rename src/drivers/hardware_specific/esp32/{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} (84%) create mode 100644 src/drivers/hardware_specific/esp32/mcpwm_private.h diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..9b07d353 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -47,7 +47,8 @@ int LowsideCurrentSense::init(){ // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(driver->params, params); + void* r = _driverSyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); // set the initialized flag diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7862b708..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -59,7 +59,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params); * function syncing the Driver with the ADC for the LowSide Sensing * @param driver_params - driver parameter structure - hardware specific * @param cs_params - current sense parameter structure - hardware specific + * + * @return void* - returns the pointer to the current sense parameter structure (unchanged) + * - returns SIMPLEFOC_CURRENT_SENSE_INIT_FAILED if the init fails */ -void _driverSyncLowSide(void* driver_params, void* cs_params); +void* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 4a37ddb8..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 357b35b0..f76c003e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..6081f87e --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,189 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +// check the version of the ESP-IDF +#include "esp_idf_version.h" + +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif + + +#include "esp32_adc_driver.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#include +#include + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" +#endif + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + + + +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("CS", str);\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + +typedef struct ESP32MCPWMCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32MCPWMCurrentSenseParams; + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; + } + // not found + return 0; +} + + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + int no_adc_channels = 0; + if( _isset(pinA) ){ + pinMode(pinA, INPUT); + params->pins[no_adc_channels++] = pinA; + } + if( _isset(pinB) ){ + pinMode(pinB, INPUT); + params->pins[no_adc_channels++] = pinB; + } + if( _isset(pinC) ){ + pinMode(pinC, INPUT); + params->pins[no_adc_channels++] = pinC; + } + + t->user_data = params; + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; + return params; +} + +void* _driverSyncLowSide(void* driver_params, void* cs_params){ +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(19, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the callback for the low side current sensing + // mcpwm_timer_event_callbacks_t can be used to set the callback + // for three timer events + // - on_full - low-side + // - on_empty - high-side + // - on_sync - sync event (not used with simplefoc) + auto cbs = mcpwm_timer_event_callbacks_t{ + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,1); //cca 250ns for on+off +#endif + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // sample the phase currents one at a time + // adc read takes around 10us which is very long + // so we are sampling one phase per call + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,0); //cca 250ns for on+off +#endif + return true; + }, + }; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupt callback."); + // set the timer state to init (so that we can call the `mcpwm_timer_register_event_callbacks` ) + // this is a hack, as this function is not supposed to be called when the timer is running + // the timer does not really go to the init state! + t->fsm = MCPWM_TIMER_FSM_INIT; + // set the callback + CHECK_CS_ERR(mcpwm_timer_register_event_callbacks(t, &cbs, cs_params), "Failed to set low side callback"); + // set the timer state to enabled again + t->fsm = MCPWM_TIMER_FSM_ENABLE; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupts."); + CHECK_CS_ERR(esp_intr_enable(t->intr), "Failed to enable low-side interrupts!"); + + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index d334008e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,165 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 - -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#include -#include - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; -} ESP32MCPWMCurrentSenseParams; - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - - - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; - } - // not found - return 0; -} - -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; - - return params; -} - - -void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 31cbb027..a212d57b 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..58545b17 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -34,8 +34,8 @@ __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, con } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ _UNUSED(driver_params); - _UNUSED(cs_params); + return cs_params; } __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d84c2fd5..d2ed881b 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -86,7 +86,7 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p // }; -// void _driverSyncLowSide(void* driver_params, void* cs_params) { +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { // // nothing to do // }; diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index da5ba85b..046f3db4 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -54,14 +54,15 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide(void* driver_params, void* cs_params) +void* _driverSyncLowSide(void* driver_params, void* cs_params) { _UNUSED(driver_params); - _UNUSED(cs_params); SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts + + return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8456759c..8cfda6d7 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -148,7 +148,7 @@ void DMA1_Channel2_IRQHandler(void) { } } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -169,6 +169,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 5f090c20..327c5305 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -48,7 +48,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -97,6 +97,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 6388e8e0..21ac7555 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -40,7 +40,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -86,6 +86,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index 3040ea46..458ec638 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -69,6 +69,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index fb32d36b..7f9fd371 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -123,6 +123,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 1fb0ab6b..e5c9470f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -41,7 +41,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -120,6 +120,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 70815d0f..3a542b53 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -197,7 +197,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p } // sync driver and the adc -void _driverSyncLowSide(void* driver_params, void* cs_params){ +void* _driverSyncLowSide(void* driver_params, void* cs_params){ Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; @@ -238,6 +238,11 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; #endif + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return cs_params; } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 87f78788..b76d17c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -245,7 +245,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; uint8_t no_operators = 3; // use 3 comparators one per pair of pwms - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); @@ -254,9 +254,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, #if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with hardware dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_operators; i++) { @@ -265,7 +265,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, } int no_generators = 6; // one per pwm - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_generators) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); // Create and configure generators mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_generators; i++) { @@ -274,12 +274,12 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring Center-Aligned 6 pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[i]); } // only available for 6pwm - SIMPLEFOC_ESP32_DEBUG("Configuring dead-time."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; mcpwm_dead_time_config_t dt_config_high; dt_config_high.posedge_delay_ticks = dead_time; @@ -294,9 +294,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with software dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); int no_pins = 6; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -305,7 +305,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -314,19 +314,19 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); params->dead_zone = dead_zone; // save the configuration variables for later group_pins_used[mcpwm_group] = 6; @@ -374,11 +374,11 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int }else{ // we will use an already instantiated timer params->timers[0] = timers[mcpwm_group][timer_no]; - SIMPLEFOC_ESP32_DEBUG("Using previously configured timer: " + String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Using previously configured timer: " + String(timer_no)); // but we cannot change its configuration without affecting the other drivers // so let's first verify that the configuration is the same if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ - SIMPLEFOC_ESP32_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + SIMPLEFOC_ESP32_DRV_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); return SIMPLEFOC_DRIVER_INIT_FAILED; } CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); @@ -387,7 +387,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } uint8_t no_operators = ceil(no_pins / 2.0); - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; for (int i = 0; i < no_operators; i++) { if (shared_timer && i == 0) { // first operator already configured @@ -400,7 +400,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // save the last operator in this group last_operator[mcpwm_group] = params->oper[no_operators - 1]; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -409,7 +409,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -419,19 +419,19 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer if not shared if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); // start the timer CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); // save the configuration variables for later params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; group_pins_used[mcpwm_group] += no_pins; @@ -440,8 +440,17 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // function setting the duty cycle to the MCPWM pin void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); - mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +// function setting the duty cycle to the MCPWM pin +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ + // phase state can be forced + // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // TODO verify with ACTIVE_HIGH/ACTIVE_LOW flags + mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); + mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index d0b7933e..446d2f25 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -11,7 +11,7 @@ #include "esp_idf_version.h" // version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif @@ -32,14 +32,17 @@ typedef struct ESP32MCPWMDriverParams { } ESP32MCPWMDriverParams; -#define SIMPLEFOC_ESP32_DEBUG(str)\ - SimpleFOCDebug::println( "ESP32-DRV: " + String(str));\ +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); + +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ // macro for checking the error of the mcpwm functions // if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED #define CHECK_ERR(func_call, message) \ if ((func_call) != ESP_OK) { \ - SIMPLEFOC_ESP32_DEBUG("ERROR - " + String(message)); \ + SIMPLEFOC_ESP32_DRV_DEBUG("ERROR - " + String(message)); \ return SIMPLEFOC_DRIVER_INIT_FAILED; \ } @@ -143,5 +146,13 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * function setting the phase state + * @param generator_high - mcpwm generator handle for the high side + * @param generator_low - mcpwm generator handle for the low side + * @param phase_state - phase state + */ +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 3c413725..dc667ab3 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -11,7 +11,7 @@ // version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp similarity index 84% rename from src/drivers/hardware_specific/esp32/esp32_mcu.cpp rename to src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 33f9db20..e2c621c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -15,10 +15,10 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { int group, timer; if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 1PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[1] = {pinA}; return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); @@ -35,17 +35,17 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int group, timer; int ret = _findBestGroup(2, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 2PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 2PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ // configure the 2pwm on only one group - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[2] = {pinA, pinB}; return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM as two 1PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -53,10 +53,10 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int pins[2][1] = {{pinA}, {pinB}}; for(int i =0; i<2; i++){ int timer = _findLastTimer(i); //find last created timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 1PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 1PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -87,10 +87,10 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i int group, timer; if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 3PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[3] = {pinA, pinB, pinC}; return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); @@ -108,16 +108,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int group, timer; int ret = _findBestGroup(4, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 4PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[4] = {pinA, pinB, pinC, pinD}; return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM as two 2PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -125,10 +125,10 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; for(int i =0; i<2; i++){ int timer = _findNextTimer(i); //find next available timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 2PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 2PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -157,10 +157,10 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons int group, timer; if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 6PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); @@ -207,6 +207,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); + + // set the phase state + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[0], ((ESP32MCPWMDriverParams*)params)->generator[1], phase_state[0]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[2], ((ESP32MCPWMDriverParams*)params)->generator[3], phase_state[1]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[4], ((ESP32MCPWMDriverParams*)params)->generator[5], phase_state[2]); #else uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h new file mode 100644 index 00000000..e133a710 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,78 @@ +/* + * This is a private declaration of different MCPWM structures and types. + * It has been copied from: https://github.com/espressif/esp-idf/blob/v5.1.4/components/driver/mcpwm/mcpwm_private.h + * + * extracted by askuric 16.06.2024 + * + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MCPWM_PRIVATE_H +#define MCPWM_PRIVATE_H + + +#include "freertos/FreeRTOS.h" +#include "esp_intr_alloc.h" +#include "esp_heap_caps.h" +#include "esp_pm.h" +#include "soc/soc_caps.h" +#include "hal/mcpwm_hal.h" +#include "hal/mcpwm_types.h" +#include "driver/mcpwm_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct mcpwm_group_t mcpwm_group_t; +typedef struct mcpwm_timer_t mcpwm_timer_t; +typedef struct mcpwm_cap_timer_t mcpwm_cap_timer_t; +typedef struct mcpwm_oper_t mcpwm_oper_t; +typedef struct mcpwm_gpio_fault_t mcpwm_gpio_fault_t; +typedef struct mcpwm_gpio_sync_src_t mcpwm_gpio_sync_src_t; +typedef struct mcpwm_timer_sync_src_t mcpwm_timer_sync_src_t; + +struct mcpwm_group_t { + int group_id; // group ID, index from 0 + int intr_priority; // MCPWM interrupt priority + mcpwm_hal_context_t hal; // HAL instance is at group level + portMUX_TYPE spinlock; // group level spinlock + uint32_t prescale; // group prescale + uint32_t resolution_hz; // MCPWM group clock resolution: clock_src_hz / clock_prescale = resolution_hz + esp_pm_lock_handle_t pm_lock; // power management lock + soc_module_clk_t clk_src; // peripheral source clock + mcpwm_cap_timer_t *cap_timer; // mcpwm capture timers + mcpwm_timer_t *timers[SOC_MCPWM_TIMERS_PER_GROUP]; // mcpwm timer array + mcpwm_oper_t *operators[SOC_MCPWM_OPERATORS_PER_GROUP]; // mcpwm operator array + mcpwm_gpio_fault_t *gpio_faults[SOC_MCPWM_GPIO_FAULTS_PER_GROUP]; // mcpwm fault detectors array + mcpwm_gpio_sync_src_t *gpio_sync_srcs[SOC_MCPWM_GPIO_SYNCHROS_PER_GROUP]; // mcpwm gpio sync array +}; + +typedef enum { + MCPWM_TIMER_FSM_INIT, + MCPWM_TIMER_FSM_ENABLE, +} mcpwm_timer_fsm_t; + +struct mcpwm_timer_t { + int timer_id; // timer ID, index from 0 + mcpwm_group_t *group; // which group the timer belongs to + mcpwm_timer_fsm_t fsm; // driver FSM + portMUX_TYPE spinlock; // spin lock + intr_handle_t intr; // interrupt handle + uint32_t resolution_hz; // resolution of the timer + uint32_t peak_ticks; // peak ticks that the timer could reach to + mcpwm_timer_sync_src_t *sync_src; // timer sync_src + mcpwm_timer_count_mode_t count_mode; // count mode + mcpwm_timer_event_cb_t on_full; // callback function when MCPWM timer counts to peak value + mcpwm_timer_event_cb_t on_empty; // callback function when MCPWM timer counts to zero + mcpwm_timer_event_cb_t on_stop; // callback function when MCPWM timer stops + void *user_data; // user data which would be passed to the timer callbacks +}; + +#ifdef __cplusplus +} +#endif + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file From 4c25ef039a4811882c3bf20014880f2cce66b986 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 22:56:59 +0200 Subject: [PATCH 184/220] forgotten mcpwm ifdef --- src/drivers/hardware_specific/esp32/mcpwm_private.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h index e133a710..dbf48970 100644 --- a/src/drivers/hardware_specific/esp32/mcpwm_private.h +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -13,6 +13,8 @@ #define MCPWM_PRIVATE_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + #include "freertos/FreeRTOS.h" #include "esp_intr_alloc.h" #include "esp_heap_caps.h" @@ -75,4 +77,6 @@ struct mcpwm_timer_t { } #endif +#endif + #endif /* MCPWM_PRIVATE_H */ \ No newline at end of file From 67248d8825cdb46755e76d32adcea3c096e80605 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 23:02:47 +0200 Subject: [PATCH 185/220] forgotten return statement change in stm32 --- .../hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 21ac7555..6b597d4e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -45,7 +45,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index 458ec638..f5ca70f3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -39,7 +39,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 7f9fd371..9c73f6d7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -47,7 +47,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index e5c9470f..5de6432a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -46,7 +46,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); From 69eb707e152a6d09b0fed78660d5ca272ea64d1d Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 23:06:14 +0200 Subject: [PATCH 186/220] forgotten bluepill --- .../hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 327c5305..49f2f3d5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -53,7 +53,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); From bbd801758a9cf7508763a2eee2cdbfdb2bf3b48a Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 08:26:11 +0200 Subject: [PATCH 187/220] added the support for phase state for 6pwm in haredware-6pwm mode + returning error if no low-side --- .../hardware_specific/generic_mcu.cpp | 10 +- .../esp32/esp32_driver_mcpwm.cpp | 223 +++++++++++------- .../esp32/esp32_driver_mcpwm.h | 2 +- 3 files changed, 145 insertions(+), 90 deletions(-) diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 58545b17..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../communication/SimpleFOCDebug.h" // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ @@ -24,13 +25,15 @@ __attribute__((weak)) void* _configureADCInline(const void* driver_params, cons // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ - return _readADCVoltageInline(pinA, cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return 0.0; } // Configure low side for generic mcu // cannot do much but __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - return _configureADCInline(driver_params, pinA, pinB, pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // sync driver and the adc @@ -38,4 +41,7 @@ __attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_par _UNUSED(driver_params); return cs_params; } + +// function starting the ADC conversion for the high side current sensing +// only necessary for certain types of MCUs __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index b76d17c5..74ed4c74 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -1,57 +1,101 @@ + /* - * MCPWM in Espressif v5.x has - * - 2x groups (units) - * each one has - * - 3 timers - * - 3 operators (that can be associated with any timer) - * which control a 2xPWM signals - * - 1x comparator + 1x generator per PWM signal for independent mode - * - 1x comparator + 2x generator per pair or PWM signals for complementary mode - * - * Independent mode: - * ------------------ - * 6 PWM independent signals per unit - * unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) - * - * group | timer | operator | comparator | generator | pwm - * -------------------------------------------------------------------------------- - * 0-1 | 0-2 | 0 | 0 | 0 | A - * 0-1 | 0-2 | 0 | 1(0 complementary) | 1 | B - * 0-1 | 0-2 | 1 | 0 | 0 | A - * 0-1 | 0-2 | 1 | 1(0 complementary) | 1 | B - * 0-1 | 0-2 | 2 | 0 | 0 | A - * 0-1 | 0-2 | 2 | 1(0 complementary) | 1 | B - * - * Complementary mode - * ------------------ - * - : 3 pairs of complementary PWM signals per unit - * unit(0/1) > timer(0) > operator(0-2) > comparator(0) > generator(0-1) > pwm(A-B pair) - * - * group | timer | operator | comparator | generator | pwm - * ------------------------------------------------------------------------ - * 0-1 | 0 | 0 | 0 | 0 | A - * 0-1 | 0 | 0 | 0 | 1 | B - * 0-1 | 0 | 1 | 0 | 0 | A - * 0-1 | 0 | 1 | 0 | 1 | B - * 0-1 | 0 | 2 | 0 | 0 | A - * 0-1 | 0 | 2 | 0 | 1 | B - * - * More info - * ---------- - * - timers can be associated with any operator, and multiple operators can be associated with the same timer - * - comparators can be associated with any operator - * - two comparators per operator for independent mode - * - one comparator per operator for complementary mode - * - generators can be associated with any comparator - * - one generator per PWM signal for independent mode - * - two generators per pair of PWM signals for complementary mode - * - dead-time can be set for each generator pair in complementary mode - * - * Docs - * ------- - * More info here: https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm - * and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +* MCPWM in espressif v5.x has +* - 2x groups (units) +* each one has +* - 3 timers +* - 3 operators (that can be associated with any timer) +* which control a 2xPWM signals +* - 1x comparator + 1x generator per PWM signal + + +* Independent mode: +* ------------------ +* 6 PWM independent signals per unit +* unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* -------------------------------------------------------------------------------- +* 0-1 | 0-2 | 0 | 0 | 0 | A +* 0-1 | 0-2 | 0 | 1 | 1 | B +* 0-1 | 0-2 | 1 | 0 | 0 | A +* 0-1 | 0-2 | 1 | 1 | 1 | B +* 0-1 | 0-2 | 2 | 0 | 0 | A +* 0-1 | 0-2 | 2 | 1 | 1 | B +* +* ------------------------------------- Example 3PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 └─ operator 1 - comparator 0 - generator 0 - pwm C +* +* ------------------------------------- Example 2PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* unit - timer 0-2 - operator 0 -| +* 0-1 └─ comparator 1 - generator 1 -> pmw B +* +* -------------------------------------- Example 4PWM ----------------------------- +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 | ┌─ comparator 0 - generator 0 -> pwm C +* └─ operator 1 -| +* └─ comparator 0 - generator 0 -> pwm D + + +* Complementary mode +* ------------------ +* - : 3 pairs of complementary PWM signals per unit +* unit(0/1) > timer(0) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(high/low pair) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* ------------------------------------------------------------------------ +* 0-1 | 0 | 0 | 0 | 0 | A +* 0-1 | 0 | 0 | 1 | 1 | B +* 0-1 | 0 | 1 | 0 | 0 | A +* 0-1 | 0 | 1 | 1 | 1 | B +* 0-1 | 0 | 2 | 0 | 0 | A +* 0-1 | 0 | 2 | 1 | 1 | B +* +* -------------------------------------- Example 6PWM ----------------------------- +* +* ┌─ comparator 0 - generator 0 -> pwm A_h +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw A_l +* | +* unit | ┌─ comparator 0 - generator 0 -> pwm B_h +* (group) - timer 0 -|- operator 1 -| +* 0-1 | └─ comparator 1 - generator 1 -> pmw B_l +* | +* | ┌─ comparator 0 - generator 0 -> pwm C_h +* └─ operator 2 -| +* └─ comparator 1 - generator 1 -> pmw C_l +* + + +* More info +* ---------- +* - timers can be associated with any operator, and multiple operators can be associated with the same timer +* - comparators can be associated with any operator +* - two comparators per operator for independent mode +* - one comparator per operator for complementary mode +* - generators can be associated with any comparator +* - one generator per PWM signal for independent mode +* - two generators per pair of PWM signals for complementary mode (not used in simplefoc) +* - dead-time can be set for each generator pair in complementary mode +* +* Docs +* ------- +* More info here: https:*www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm +* and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html */ + #include "../../hardware_api.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -195,14 +239,14 @@ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ // configuring center aligned pwm // More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low -void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ if(inverted) - mcpwm_generator_set_actions_on_compare_event(gena, + return mcpwm_generator_set_actions_on_compare_event(gena, MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), MCPWM_GEN_COMPARE_EVENT_ACTION_END()); else - mcpwm_generator_set_actions_on_compare_event(gena, + return mcpwm_generator_set_actions_on_compare_event(gena, MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), MCPWM_GEN_COMPARE_EVENT_ACTION_END()); @@ -247,6 +291,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, uint8_t no_operators = 3; // use 3 comparators one per pair of pwms SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); @@ -263,6 +310,21 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } + +#else // software dead-time (software 6pwm) +// software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); + + int no_pins = 6; + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } +#endif int no_generators = 6; // one per pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); @@ -273,10 +335,14 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int oper_index = (int)floor(i / 2); CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); - for (int i = 0; i < 3; i++) { - _configureCenterAlign(params->generator[2*i],params->comparator[i]); + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); + } // only available for 6pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); @@ -289,37 +355,17 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, dt_config_low.posedge_delay_ticks = 0; dt_config_low.negedge_delay_ticks = dead_time; dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); - CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i+1], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); - int no_pins = 6; - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); - // Create and configure comparators - mcpwm_comparator_config_t comparator_config = {0}; - for (int i = 0; i < no_pins; i++) { - int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); - CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); - } - - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); - // Create and configure generators; - mcpwm_generator_config_t generator_config = {}; - for (int i = 0; i < no_pins; i++) { - generator_config.gen_gpio_num = pins[i]; - int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); - } - - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < 3; i++) { - _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); - _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); } #endif + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); @@ -389,6 +435,9 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int uint8_t no_operators = ceil(no_pins / 2.0); SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; for (int i = 0; i < no_operators; i++) { if (shared_timer && i == 0) { // first operator already configured params->oper[0] = last_operator[mcpwm_group]; @@ -421,7 +470,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { - _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); + CHECK_ERR(_configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH), "Failed to configure center align pwm: " + String(i)); } SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); @@ -446,9 +495,9 @@ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_c // function setting the duty cycle to the MCPWM pin void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ - // phase state can be forced - // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions - // TODO verify with ACTIVE_HIGH/ACTIVE_LOW flags + // phase state is forced in hardware pwm mode + // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // github issue: https://github.com/espressif/esp-idf/issues/12237 mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 446d2f25..5726e906 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -103,7 +103,7 @@ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); * @param cmpa - mcpwm comparator handle * @param inverted - true if the signal is inverted, false otherwise */ -void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); /** * function calculating the pwm period From 63fda20fb51a056507a6907a212210771f40e080 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 08:26:48 +0200 Subject: [PATCH 188/220] added workflow or s2 --- .github/workflows/esp32.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index e0899bf1..db551b41 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -19,6 +19,7 @@ jobs: - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 - esp32:esp32:esp32s3 # esp32s3 + - esp32:esp32:esp32c3 # esp32c3 include: @@ -30,6 +31,10 @@ jobs: platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32c3 # esp32c3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino From 3c3e9c8cf67ecbdd05dcc3653ead4c651e1907b2 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 09:11:22 +0200 Subject: [PATCH 189/220] added the debugging in examples --- .../B_G431B_ESC1/B_G431B_ESC1.ino | 9 ++++++--- .../bluepill_position_control.ino | 8 ++++++-- .../bluepill_position_control.ino | 8 ++++++-- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../esp32_current_control_low_side.ino | 10 ++++++---- .../stm32_current_control_low_side.ino | 10 ++++++---- .../teensy4_current_control_low_side.ino | 10 ++++++---- .../esp32_position_control/esp32_position_control.ino | 9 ++++++--- .../esp32_position_control/esp32_position_control.ino | 8 ++++++-- .../position_control/position_control.ino | 6 +++--- .../odrive_example_encoder/odrive_example_encoder.ino | 8 ++++++-- .../odrive_example_spi/odrive_example_spi.ino | 8 ++++++-- .../nano33IoT_velocity_control.ino | 5 +++++ .../single_full_control_example.ino | 9 ++++++--- .../SimpleFOCMini/angle_control/angle_control.ino | 9 ++++++--- .../double_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../double_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../smartstepper_control/smartstepper_control.ino | 8 ++++++-- .../open_loop_velocity_6pwm.ino | 7 ++++++- .../open_loop_velocity_6pwm.ino | 7 ++++++- .../open_loop_position_example.ino | 7 ++++++- .../open_loop_velocity_example.ino | 7 ++++++- .../encoder/angle_control/angle_control.ino | 9 ++++++--- .../magnetic_sensor/angle_control/angle_control.ino | 10 +++++++--- .../encoder/current_control/current_control.ino | 8 ++++++-- .../encoder/voltage_control/voltage_control.ino | 8 ++++++-- .../hall_sensor/voltage_control/voltage_control.ino | 8 ++++++-- .../voltage_control/voltage_control.ino | 8 ++++++-- .../hall_sensor/velocity_control/velocity_control.ino | 8 ++++++-- .../velocity_control/velocity_control.ino | 8 ++++++-- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 6 +++++- .../osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino | 5 +++++ .../alignment_and_cogging_test.ino | 5 ++++- .../encoder/find_kv_rating/find_kv_rating.ino | 8 ++++++-- .../hall_sensor/find_kv_rating/find_kv_rating.ino | 10 +++++++--- .../magnetic_sensor/find_kv_rating/find_kv_rating.ino | 8 ++++++-- .../find_pole_pairs_number/find_pole_pairs_number.ino | 8 ++++++-- .../find_pole_pairs_number/find_pole_pairs_number.ino | 9 ++++++--- .../find_sensor_offset_and_direction.ino | 7 ++++++- .../commander_tune_custom_loop.ino | 7 ++++++- .../step_dir_motor_example/step_dir_motor_example.ino | 10 +++++++--- .../generic_current_sense/generic_current_sense.ino | 9 ++++++++- .../inline_current_sense_test.ino | 8 +++++++- .../bldc_driver_3pwm_standalone.ino | 6 ++++++ .../bldc_driver_6pwm_standalone.ino | 6 ++++++ .../stepper_driver_2pwm_standalone.ino | 6 ++++++ .../stepper_driver_4pwm_standalone.ino | 6 ++++++ 54 files changed, 322 insertions(+), 112 deletions(-) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 8d751742..f1eeefd2 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -26,6 +26,12 @@ void doTarget(char* cmd) { command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -76,9 +82,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index be853f0e..b29e45d8 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB, doI); @@ -75,8 +81,6 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index e27f1e7a..caef7ffe 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -36,6 +36,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -72,8 +78,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 842f383a..3d90db16 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -43,6 +43,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -90,9 +96,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 2b4cf6f1..af56e067 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -46,6 +46,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -91,9 +97,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 84033b42..7d7fe14f 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino index 3e3f04ab..e1b4c392 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino index 841134d8..c9b4516f 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -54,6 +54,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -116,10 +122,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index f6dac940..7f6b33ce 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -24,6 +24,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -66,9 +72,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index f025895b..9ba9604a 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -69,8 +75,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 5b5c8e40..d9107654 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -42,6 +42,9 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // initialise encoder hardware encoder.init(); // interrupt initialization @@ -85,9 +88,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino index b89c215f..504ab9c8 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -70,6 +70,12 @@ void doI(){encoder.handleIndex();} void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -96,8 +102,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino index 2aed6bfb..7abcde54 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -69,6 +69,12 @@ SPIClass SPI_3(SPI3_MOSO, SPI3_MISO, SPI3_SCL); void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -94,8 +100,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index 70ec28b0..650050ef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -23,7 +23,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(1000); Serial.println("Initializing..."); diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index eb52f51b..bc387437 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -21,6 +21,12 @@ void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -56,9 +62,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino index 9102c89a..8eb122a0 100644 --- a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -38,6 +38,12 @@ Commander command = Commander(Serial); void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // if SimpleFOCMini is stacked in arduino headers // on pins 12,11,10,9,8 // pin 12 is used as ground @@ -84,9 +90,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index c048956b..fb5e1563 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -28,6 +28,12 @@ void doMotor2(char* cmd){ command.motor(&motor2, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -72,9 +78,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 10e26c9f..3faa38b2 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -17,6 +17,12 @@ void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -48,9 +54,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index b40ab62a..8eb3a86d 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -39,6 +39,12 @@ void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -88,9 +94,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 76a7296a..b65a9cb0 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 02593286..9c80a36f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino index 80f2fe64..18047108 100644 --- a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -18,6 +18,12 @@ void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&SerialUSB); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -31,8 +37,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with SerialUSB - SerialUSB.begin(115200); // comment out if not needed motor.useMonitoring(SerialUSB); diff --git a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 40924ee3..9143dde7 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -18,6 +18,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -48,7 +54,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 9c6eea03..5ff53311 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -33,6 +33,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -63,7 +69,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 5a915dd8..6bcfdff9 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -23,6 +23,12 @@ void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -53,7 +59,6 @@ void setup() { command.add('L', doLimit, "voltage limit"); command.add('V', doLimit, "movement velocity"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target position [rad]"); _delay(1000); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 43fc6f73..075dc9c6 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -23,6 +23,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -50,7 +56,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index ccb3980e..03e6ea75 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -48,6 +48,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -88,9 +94,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 752030c9..f0f9b27c 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -31,6 +31,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -67,9 +73,7 @@ void setup() { motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index c9123401..82aec86a 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -27,6 +27,12 @@ void doTarget(char* cmd) { command.scalar(&target_current, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -70,8 +76,6 @@ void setup() { // motor.LPF_current_q.Tf = 0.002f; // 1ms default // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 66ff4569..219637c9 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,8 +61,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 7273d156..c72c9224 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -35,6 +35,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -57,8 +63,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index d869f4dd..dbb56080 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -30,6 +30,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,8 +53,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino index 1d39173a..4fb47f02 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino @@ -47,6 +47,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize sensor sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -84,8 +90,6 @@ void setup() { // velocity low pass filtering time constant motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 98e09425..40299b8f 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,8 +72,6 @@ void setup() { // the lower the less filtered motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 186d257e..d6ddc6b4 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -34,6 +34,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -68,9 +74,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index df7b76ac..ff6fe7f3 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -41,6 +41,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -76,9 +82,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 098f6888..e863d8c9 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -33,6 +33,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,9 +72,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index afb95294..14d4867c 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -67,8 +67,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); Commander command = Commander(Serial); void setup() { + // use monitoring with serial Serial.begin(115200); - + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + WiFi.begin(ssid, pass); Serial.print("Connecting WiFi "); diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 7c6aa4f0..940b59c2 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -104,7 +104,12 @@ void setup() { digitalWrite(26, 0); digitalWrite(27, 0); + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(200); WiFi.begin(ssid, pass); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 7324edf0..9c523c22 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -80,8 +80,11 @@ void testAlignmentAndCogging(int direction) { void setup() { + // use monitoring with serial Serial.begin(115200); - while (!Serial) ; + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); // driver config driver.voltage_power_supply = 12; diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index c304d195..c39657b1 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -42,6 +42,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); @@ -62,8 +68,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 575968f4..4eadd376 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -39,6 +39,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -58,9 +64,7 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index a9d29d86..1df631af 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -37,6 +37,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); // link the motor to the sensor @@ -56,8 +62,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index aac879cd..ad8e69ce 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -32,6 +32,12 @@ void doB(){encoder.handleB();} void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -48,8 +54,6 @@ void setup() { // initialize motor motor.init(); - // monitoring port - Serial.begin(115200); // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index e98a4529..b44bc0bb 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -33,6 +33,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,9 +53,6 @@ void setup() { // initialize motor hardware motor.init(); - // monitoring port - Serial.begin(115200); - // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); Serial.println("-\n"); diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 27163dfb..407469fc 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -29,6 +29,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // power supply voltage driver.voltage_power_supply = 12; driver.init(); @@ -54,7 +60,6 @@ void setup() { motor.initFOC(); - Serial.begin(115200); Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); diff --git a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino index cbeb6ecb..074538ad 100644 --- a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino +++ b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -29,6 +29,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -46,7 +52,6 @@ void setup() { motor.controller = MotionControlType::torque; // use monitoring with serial - Serial.begin(115200); motor.useMonitoring(Serial); // initialize motor motor.init(); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index f8978577..4cd87970 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -24,6 +24,12 @@ void onStep() { step_dir.handle(); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -64,9 +70,7 @@ void setup() { motor.P_angle.P = 10; // maximal velocity of the position control motor.velocity_limit = 100; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino index e999de23..2448a51c 100644 --- a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -31,6 +31,14 @@ GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCu void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if callbacks are not provided in the constructor // they can be assigned directly: //current_sense.readCallback = readCurrentSense; @@ -40,7 +48,6 @@ void setup() { current_sense.init(); - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index c1a5139b..195021eb 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -11,13 +11,19 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise the current sensing current_sense.init(); // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 1235fccd..82716353 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -7,6 +7,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 478d3974..1ed37441 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -6,6 +6,12 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 4627efa9..889143e2 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -13,6 +13,12 @@ StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index e028a737..95dcc589 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -8,6 +8,12 @@ StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable From 52e8d9df049c82f6457ef37303b90beb6efc6327 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 13:59:59 +0200 Subject: [PATCH 190/220] added the support for esp32s3 to read all the current phases in one interrupt --- .../esp32/esp32_mcpwm_mcu.cpp | 45 +++++++++++++++---- .../esp32/esp32_driver_mcpwm.cpp | 2 +- 2 files changed, 38 insertions(+), 9 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6081f87e..6e25e164 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -22,8 +22,28 @@ #include #include +#define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG #include "driver/gpio.h" + + +// if the MCU is not ESP32S3, the ADC read time is too long to +// sample all three phase currents in one interrupt +// so we will sample one phase per interrupt +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT +#endif + + +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define DEBUGPIN 16 +#define GPIO_NUM GPIO_NUM_16 +#else +#define DEBUGPIN 19 +#define GPIO_NUM GPIO_NUM_19 +#endif + #endif #define _ADC_VOLTAGE 3.3f @@ -132,9 +152,11 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p return params; } + + void* _driverSyncLowSide(void* driver_params, void* cs_params){ #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - pinMode(19, OUTPUT); + pinMode(DEBUGPIN, OUTPUT); #endif ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; @@ -150,22 +172,29 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // mcpwm_timer_event_callbacks_t can be used to set the callback // for three timer events // - on_full - low-side - // - on_empty - high-side + // - on_empty - high-side // - on_sync - sync event (not used with simplefoc) auto cbs = mcpwm_timer_event_callbacks_t{ .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - gpio_set_level(GPIO_NUM_19,1); //cca 250ns for on+off +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif + +#ifdef SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT // sample the phase currents one at a time + // ex. ESP32's adc read takes around 10us which is very long // increment buffer index p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; - // sample the phase currents one at a time - // adc read takes around 10us which is very long // so we are sampling one phase per call p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - gpio_set_level(GPIO_NUM_19,0); //cca 250ns for on+off +#else // sample all available phase currents at once + // ex. ESP32S3's adc read takes around 1us which is good enough + for(int i=0; i < p->no_adc_channels; i++) + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); +#endif + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off #endif return true; }, diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 74ed4c74..4ab02993 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -489,7 +489,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // function setting the duty cycle to the MCPWM pin void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); + float duty = _constrain(duty_cycle, 0.0, 1.0); mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); } From 910c24ff7e26007235b77a6c8a6a16ae43c454ec Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 14:03:09 +0200 Subject: [PATCH 191/220] forgotten change for bg431 --- src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8cfda6d7..46cb20be 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -27,7 +27,7 @@ volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the resul // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin, const void* cs_params){ +float _readADCVoltageLowSide(const int pin, const void* cs_params){ uint32_t raw_adc = 0; if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[1]; From 220050447102d58134c766a8e12a5dca90f1b539 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 14:21:37 +0200 Subject: [PATCH 192/220] s3 bugfix - it cannot read faster than 10us per adc sample --- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6e25e164..49f549dc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -28,14 +28,6 @@ #include "driver/gpio.h" -// if the MCU is not ESP32S3, the ADC read time is too long to -// sample all three phase currents in one interrupt -// so we will sample one phase per interrupt -#ifdef CONFIG_IDF_TARGET_ESP32S3 -#define SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT -#endif - - #ifdef CONFIG_IDF_TARGET_ESP32S3 #define DEBUGPIN 16 #define GPIO_NUM GPIO_NUM_16 @@ -181,17 +173,12 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif -#ifdef SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT // sample the phase currents one at a time - // ex. ESP32's adc read takes around 10us which is very long + // sample the phase currents one at a time + // ESP's adc read takes around 10us which is very long // increment buffer index p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; // so we are sampling one phase per call p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#else // sample all available phase currents at once - // ex. ESP32S3's adc read takes around 1us which is good enough - for(int i=0; i < p->no_adc_channels; i++) - p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#endif #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off From e92567e941aad2b989ff61a5b56bedf4ccfd536e Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 15:41:23 +0200 Subject: [PATCH 193/220] bugfix for #295 and #320 + added #346 --- .../hardware_specific/esp32/esp32s_adc_driver.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a212d57b..a8541568 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -111,7 +111,7 @@ void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) return ; } __analogInit(); - if(channel > 7){ + if(channel > 9){ SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); } else { SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); @@ -152,9 +152,8 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ } if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); @@ -171,7 +170,7 @@ bool IRAM_ATTR __adcBusy(uint8_t pin){ return false;//not adc pin } - if(channel > 7){ + if(channel > 9){ return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); } return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); @@ -185,7 +184,7 @@ uint16_t IRAM_ATTR __adcEnd(uint8_t pin) if(channel < 0){ return 0;//not adc pin } - if(channel > 7){ + if(channel > 9){ while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { @@ -223,9 +222,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) __analogInit(); if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD,(1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); @@ -235,7 +233,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) uint16_t value = 0; - if(channel > 7){ + if(channel > 9){ while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { From 0b367c37f772edf067467c6098f853d39f314c8e Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:16:55 +0200 Subject: [PATCH 194/220] refactured esp32 fast adc driver - simplified a lot --- .../esp32/esp32_adc_driver.cpp | 285 ++++++------------ .../esp32/esp32_adc_driver.h | 90 +----- .../esp32/esp32_mcpwm_mcu.cpp | 41 ++- .../esp32/esp32s_adc_driver.cpp | 256 ---------------- 4 files changed, 139 insertions(+), 533 deletions(-) delete mode 100644 src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..5ad01b2a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,89 +1,20 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} +// maybe go to the fast ADC in future even outside the MCPWM (we'd have to remove the SOC_MCPWM_SUPPORTED flag) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} +#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits +#include "soc/sens_reg.h" +// configure the ADCs in RTC mode +// saves about 3us per call +// going from 12us to 9us +void __configFastADCs(){ + + // configure both ADCs in RTC mode SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); @@ -100,59 +31,17 @@ void IRAM_ATTR __analogInit(){ SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } } -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) - | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) - | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ return false;//not adc pin } + // start teh ADC conversion if(channel > 9){ CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); @@ -162,57 +51,51 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } + if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + // return value + return value; } -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} +#elif (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) // if esp32 s2 or s3 variants + +#include "soc/sens_reg.h" + + +// configure the ADCs in RTC mode +// no real gain +// void __configFastADCs(){ + +// SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); +// SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); + +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + +// CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + +// CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); +// while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== +// } uint16_t IRAM_ATTR adcRead(uint8_t pin) { @@ -221,38 +104,66 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) return false;//not adc pin } - __analogInit(); - + // start the ADC conversion if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); } uint16_t value = 0; - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + if(channel > 9){ + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); + // read teh value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + return value; } +#else // if others just use analogRead + +uint16_t IRAM_ATTR adcRead(uint8_t pin){ + return analogRead(pin); +} + +#endif + + +// configure the ADC for the pin +bool IRAM_ATTR adcInit(uint8_t pin){ + static bool initialized = false; + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(! initialized){ + analogSetAttenuation(SIMPLEFOC_ADC_ATTEN); + analogReadResolution(SIMPLEFOC_ADC_RES); + } + pinMode(pin, ANALOG); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); + analogRead(pin); + +#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant + __configFastADCs(); +#endif + + initialized = true; + return true; +} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f76c003e..9a91d568 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,88 +1,30 @@ - - #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -/* - * Get ADC value for pin - * */ -uint16_t adcRead(uint8_t pin); - -/* - * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). - * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. - * Range is 1 - 16 - * - * Note: compatibility with Arduino SAM - */ -void __analogReadResolution(uint8_t bits); - -/* - * Sets the sample bits and read resolution - * Default is 12bit (0 - 4095) - * Range is 9 - 12 - * */ -void __analogSetWidth(uint8_t bits); - -/* - * Set number of cycles per sample - * Default is 8 and seems to do well - * Range is 1 - 255 - * */ -void __analogSetCycles(uint8_t cycles); - -/* - * Set number of samples in the range. - * Default is 1 - * Range is 1 - 255 - * This setting splits the range into - * "samples" pieces, which could look - * like the sensitivity has been multiplied - * that many times - * */ -void __analogSetSamples(uint8_t samples); - -/* - * Set the divider for the ADC clock. - * Default is 1 - * Range is 1 - 255 - * */ -void __analogSetClockDiv(uint8_t clockDiv); +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -/* - * Set the attenuation for all channels - * Default is 11db - * */ -void __analogSetAttenuation(uint8_t attenuation); - -/* - * Set the attenuation for particular pin - * Default is 11db - * */ -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); -/* - * Attach pin to ADC (will also clear any other analog mode that could be on) - * */ -bool __adcAttachPin(uint8_t pin); +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 -/* - * Start ADC conversion on attached pin's bus +/** + * Get ADC value for pin + * @param pin - pin number + * @return ADC value (0 - 4095) * */ -bool __adcStart(uint8_t pin); +uint16_t adcRead(uint8_t pin); -/* - * Check if conversion on the pin's ADC bus is currently running - * */ -bool __adcBusy(uint8_t pin); +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin + */ +bool adcInit(uint8_t pin); -/* - * Get the result of the conversion (will wait if it have not finished) - * */ -uint16_t __adcEnd(uint8_t pin); #endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ #endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 49f549dc..6760de23 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -73,16 +73,23 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } + } + return params; } @@ -123,19 +130,21 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; int no_adc_channels = 0; - if( _isset(pinA) ){ - pinMode(pinA, INPUT); - params->pins[no_adc_channels++] = pinA; - } - if( _isset(pinB) ){ - pinMode(pinB, INPUT); - params->pins[no_adc_channels++] = pinB; - } - if( _isset(pinC) ){ - pinMode(pinC, INPUT); - params->pins[no_adc_channels++] = pinC; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + int adc_pins[3] = {pinA, pinB, pinC}; + for (int i = 0; i < 3; i++){ + if(_isset(adc_pins[i])){ + if(!adcInit(adc_pins[i])){ + SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + params->pins[no_adc_channels++] = adc_pins[i]; + } } t->user_data = params; diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp deleted file mode 100644 index a8541568..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ /dev/null @@ -1,256 +0,0 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" // deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits - - SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 9){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ - - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 9){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} - -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - __analogInit(); - - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD,(1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - - uint16_t value = 0; - - if(channel > 9){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - - -#endif \ No newline at end of file From d89fa262ff40bdf9078f21d683ee05c4324e970f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:18:49 +0200 Subject: [PATCH 195/220] added the error debugging --- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6760de23..53426900 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -84,12 +84,12 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p if(_isset(params->pins[i])){ pinMode(params->pins[i], ANALOG); if(!adcInit(params->pins[i])) { - SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } } } - + return params; } @@ -109,6 +109,7 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ return p->adc_buffer[no_channel] * p->adc_voltage_conv; else no_channel++; } + SIMPLEFOC_DEBUG("ERROR: ADC pin not found in the buffer!"); // not found return 0; } @@ -126,7 +127,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p // check if low side callback is already set // if it is, return error if(t->on_full != nullptr){ - SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } @@ -140,7 +141,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p for (int i = 0; i < 3; i++){ if(_isset(adc_pins[i])){ if(!adcInit(adc_pins[i])){ - SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } params->pins[no_adc_channels++] = adc_pins[i]; @@ -165,7 +166,7 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // check if low side callback is already set // if it is, return error if(t->on_full != nullptr){ - SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } From 710edb4aa584a46900205bf96d9df68e8f8e84c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:33:37 +0200 Subject: [PATCH 196/220] more refactoring of the current sensing --- .../esp32/esp32_adc_driver.cpp | 11 ++- .../esp32/esp32_adc_driver.h | 4 - .../esp32/esp32_ledc_mcu.cpp | 27 ------- .../esp32/esp32_mcpwm_mcu.cpp | 75 +++---------------- .../hardware_specific/esp32/esp32_mcu.cpp | 43 +++++++++++ .../hardware_specific/esp32/esp32_mcu.h | 37 +++++++++ 6 files changed, 98 insertions(+), 99 deletions(-) delete mode 100644 src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.h diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 5ad01b2a..649343f1 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,8 +1,10 @@ #include "esp32_adc_driver.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#include "esp32_mcu.h" -// maybe go to the fast ADC in future even outside the MCPWM (we'd have to remove the SOC_MCPWM_SUPPORTED flag) -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 #ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant @@ -38,6 +40,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } @@ -76,7 +79,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) // configure the ADCs in RTC mode -// no real gain +// no real gain - see if we do something with it later // void __configFastADCs(){ // SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); @@ -101,6 +104,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } @@ -147,6 +151,7 @@ bool IRAM_ATTR adcInit(uint8_t pin){ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 9a91d568..f41904d8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,10 +5,6 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#define SIMPLEFOC_ADC_ATTEN ADC_11db -#define SIMPLEFOC_ADC_RES 12 - /** * Get ADC value for pin * @param pin - pin number diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp deleted file mode 100644 index 3487dbd7..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && (!defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC)) - -#include "esp32_adc_driver.h" - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); - - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 53426900..7efb6294 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -1,7 +1,5 @@ #include "../../hardware_api.h" #include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -12,22 +10,23 @@ #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif - -#include "esp32_adc_driver.h" +#include "esp32_mcu.cpp" +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" #include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -#include -#include -#define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + +// adding a debug toggle pin to measure the time of the interrupt with oscilloscope + +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG #include "driver/gpio.h" - #ifdef CONFIG_IDF_TARGET_ESP32S3 #define DEBUGPIN 16 #define GPIO_NUM GPIO_NUM_16 @@ -38,60 +37,6 @@ #endif -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - - -#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ - SIMPLEFOC_ESP32_DEBUG("CS", str);\ - -#define CHECK_CS_ERR(func_call, message) \ - if ((func_call) != ESP_OK) { \ - SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ - } - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - int adc_buffer[3] = {}; - int buffer_index = 0; - int no_adc_channels = 0; -} ESP32MCPWMCurrentSenseParams; - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - // initialize the ADC pins - // fail if the pin is not an ADC pin - for (int i = 0; i < 3; i++){ - if(_isset(params->pins[i])){ - pinMode(params->pins[i], ANALOG); - if(!adcInit(params->pins[i])) { - SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } - } - } - - return params; -} /** @@ -101,7 +46,7 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + ESP32CurrentSenseParams* p = (ESP32CurrentSenseParams*)cs_params; int no_channel = 0; for(int i=0; i < 3; i++){ if(!_isset(p->pins[i])) continue; @@ -132,7 +77,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p } - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams{}; int no_adc_channels = 0; // initialize the ADC pins @@ -178,7 +123,7 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // - on_sync - sync event (not used with simplefoc) auto cbs = mcpwm_timer_event_callbacks_t{ .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ - ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; + ESP32CurrentSenseParams *p = (ESP32CurrentSenseParams*)user_data; #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp new file mode 100644 index 00000000..9cf3a271 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -0,0 +1,43 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "esp32_adc_driver.h" +#include "esp32_mcu.h" + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((ESP32CurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } + } + + return params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h new file mode 100644 index 00000000..8966d9ef --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -0,0 +1,37 @@ +#ifndef ESP32_MCU_CURRENT_SENSING_H +#define ESP32_MCU_CURRENT_SENSING_H + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" + +#include "esp32_adc_driver.h" + + +// esp32 current sense parameters +typedef struct ESP32CurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32CurrentSenseParams; + +// macros for debugging wuing the simplefoc debug system +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-CS"+String(tag)+ ": "+ String(str));\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +#endif // ESP_H && ARDUINO_ARCH_ESP32 +#endif \ No newline at end of file From bc2f84815adf47ee08abe9741fe748756fd24f84 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:37:27 +0200 Subject: [PATCH 197/220] typo in debug macro --- src/current_sense/hardware_specific/esp32/esp32_mcu.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h index 8966d9ef..b50c3113 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.h +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -21,7 +21,7 @@ typedef struct ESP32CurrentSenseParams { // macros for debugging wuing the simplefoc debug system #define SIMPLEFOC_ESP32_CS_DEBUG(str)\ - SimpleFOCDebug::println( "ESP32-CS"+String(tag)+ ": "+ String(str));\ + SimpleFOCDebug::println( "ESP32-CS: "+ String(str));\ #define CHECK_CS_ERR(func_call, message) \ if ((func_call) != ESP_OK) { \ From a1bd20ef7ddca155ee224cdc76f81c105b90d59f Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 12:35:09 +0200 Subject: [PATCH 198/220] say the pin name if the generator fails --- src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 4ab02993..ad1ff0b2 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -333,7 +333,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, for (int i = 0; i < no_generators; i++) { generator_config.gen_gpio_num = pins[i]; int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]),"Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); @@ -464,7 +464,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int for (int i = 0; i < no_pins; i++) { generator_config.gen_gpio_num = pins[i]; int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); } From 8ce92e0692ec13a5c981c9dc4dc35eb018ce7e73 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 12:36:01 +0200 Subject: [PATCH 199/220] restructured imports --- .../hardware_specific/esp32/esp32_adc_driver.cpp | 6 ++++-- .../hardware_specific/esp32/esp32_adc_driver.h | 2 -- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 4 +--- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 7 +------ src/current_sense/hardware_specific/esp32/esp32_mcu.h | 4 ++-- 5 files changed, 8 insertions(+), 15 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 649343f1..b9594918 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,8 +1,8 @@ -#include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "esp32_mcu.h" +#include "esp32_adc_driver.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #define SIMPLEFOC_ADC_ATTEN ADC_11db #define SIMPLEFOC_ADC_RES 12 @@ -138,6 +138,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) #else // if others just use analogRead +#pragma message("SimpleFOC: Using analogRead for ADC reading, no fast ADC configuration available!") + uint16_t IRAM_ATTR adcRead(uint8_t pin){ return analogRead(pin); } diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f41904d8..688b6d5a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,8 +1,6 @@ #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#include "Arduino.h" - #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /** diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 7efb6294..33d547db 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -1,5 +1,4 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" +#include "esp32_mcu.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -10,7 +9,6 @@ #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif -#include "esp32_mcu.cpp" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" #include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 9cf3a271..e5ed3fbf 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -1,11 +1,6 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - -#include "esp32_adc_driver.h" #include "esp32_mcu.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Inline adc reading implementation diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h index b50c3113..9207fb6a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.h +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -1,12 +1,12 @@ #ifndef ESP32_MCU_CURRENT_SENSING_H #define ESP32_MCU_CURRENT_SENSING_H +#include "../../hardware_api.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_api.h" #include "esp32_adc_driver.h" From a734d0db08dea870364065c334318d46bfad37ff Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 13:13:42 +0200 Subject: [PATCH 200/220] typo in the defines --- .../hardware_specific/esp32/esp32_adc_driver.cpp | 6 +++--- .../hardware_specific/esp32/esp32_adc_driver.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index b9594918..caf29c19 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -7,7 +7,7 @@ #define SIMPLEFOC_ADC_RES 12 -#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant #include "soc/sens_reg.h" @@ -73,7 +73,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) return value; } -#elif (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) // if esp32 s2 or s3 variants +#elif CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 // if esp32 s2 or s3 variants #include "soc/sens_reg.h" @@ -165,7 +165,7 @@ bool IRAM_ATTR adcInit(uint8_t pin){ analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); analogRead(pin); -#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant __configFastADCs(); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 688b6d5a..cad441fc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,7 +1,7 @@ #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Get ADC value for pin From e81af5dbdf97e49efc0893654d179e3b03ea27f8 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:22:52 -0500 Subject: [PATCH 201/220] Fix for #415 sin/cos integer overflow on 16-bit CPUs Terrible bug in the fast sin/cos added last year, resulting in rough sine waves and a spike from +1 to -1 at pi/2 --- src/common/foc_utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fa937d15..7ae372f7 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,20 +9,21 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); - int t1, t2, frac = i & 0xff; + int32_t t1, t2; + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); + int frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { - t1 = sine_array[i]; t2 = sine_array[i+1]; + t1 = (int32_t)sine_array[i]; t2 = (int32_t)sine_array[i+1]; } else if(i < 128) { - t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + t1 = (int32_t)sine_array[128 - i]; t2 = (int32_t)sine_array[127 - i]; } else if(i < 192) { - t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; + t1 = -(int32_t)sine_array[-128 + i]; t2 = -(int32_t)sine_array[-127 + i]; } else { - t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + t1 = -(int32_t)sine_array[256 - i]; t2 = -(int32_t)sine_array[255 - i]; } return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } From bf75d206ad11d0c0e56f5074395e17f545702d69 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:32:51 -0500 Subject: [PATCH 202/220] Removed accidental addition of LinearHall.h I thought I discarded this change after moving LinearHall back to drivers, but somehow it got included anyway --- src/SimpleFOC.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 9e1c80c6..4e6815e5 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,7 +104,6 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" -#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" From 463eb31f354dd80c29f0ae4f16d0c1733f208a0a Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:22:59 +0200 Subject: [PATCH 203/220] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 86551f79..c456b1d4 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://ardubadge.simplefoc.com?lib=Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) From d2a409338f9d796c4ff86ecad0f42b576d05e522 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:40:43 +0200 Subject: [PATCH 204/220] one too many electrical angle --- src/StepperMotor.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index df710aad..38e8999f 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -301,11 +301,6 @@ void StepperMotor::loopFOC() { // if disabled do nothing if(!enabled) return; - // Needs the update() to be called first - // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() - // which is in range 0-2PI - electrical_angle = electricalAngle(); - // Needs the update() to be called first // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() // which is in range 0-2PI From eb4fd7f6ce2690f95d66e5644a4c9c04a3c179df Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:41:35 +0200 Subject: [PATCH 205/220] update of pwm duty cycle on timer zero --- src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index ad1ff0b2..a481c6ff 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -306,6 +306,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); @@ -319,6 +320,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_pins; i++) { int oper_index = (int)floor(i / 2); CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); @@ -452,6 +454,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_pins; i++) { int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); From 842b3615eec1b74cdb4729c03a2c64d21d7135d8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Jul 2024 13:15:36 +0200 Subject: [PATCH 206/220] new current sense alignement --- src/common/base_classes/CurrentSense.cpp | 283 +++++++++++++++-------- 1 file changed, 187 insertions(+), 96 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4e6c3386..e354dfaf 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -174,113 +174,204 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ - int exit_flag = 1; - if(_isset(pinA)){ - // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B + bool phases_switched = 0; + bool phases_inverted = 0; + + + // set phase A active and phases B and C down + bldc_driver->setPwm(voltage, 0, 0); + _delay(500); + PhaseCurrent_s c_a = readAverageCurrents(); + // check if currents are to low (lower than 100mA) + // TODO calculate the 100mA threshold from the ADC resolution + // if yes throw an error and return 0 + // either the current sense is not connected or the current is + // too low for calibration purposes (one should raise the motor.voltage_sensor_align) + if((_isset(pinA) && fabs(c_a.a) < 0.1f) + || (_isset(pinB) && fabs(c_a.b) < 0.1f) + || (_isset(pinC) && fabs(c_a.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I/2 on the phase B and I/2 on the phase C + + // find the highest magnitude in c_a + // and make sure it's around 2 (1.5 at least) times higher than the other two + float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!ca[i]) continue; // current not measured + if(ca[i] > max_c){ + max_c = ca[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!ca[j]) continue; // current not measured + float ratio = max_c / ca[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + + // check the current magnitude ratios + // 1) if there is one current that is approximately 2 times higher than the other two + // this is the A current + // 2) if the max current is not at least 1.5 times higher than the other two + // we have two cases: + // - either we only measure two currents and the third one is not measured - then phase A is not measured + // - or the current sense is not connected properly + + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + break; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.a, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true;// signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_a.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!"); + return 0; + }else{ //phase A is not measured so put the _NC to the phase A + if(_isset(pinA) && !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch A-(B)NC"); _swap(pinA, pinB); _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-C"); - // switch phase A and C + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + }else if(_isset(pinA) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); _swap(pinA, pinC); _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read A"); - // error in current sense - phase either not measured or bad connection - return 0; + _swap(c_a.b, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched } } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2); - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-A"); - // switch phase A and B - _swap(pinB, pinA); - _swap(offset_ib, offset_ia); - _swap(gain_b, gain_a); - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-C"); + // at this point the current sensing on phase A can be either: + // - aligned with the driver phase A + // - or the phase A is not measured and the _NC is connected to the phase A + // + // In either case A is done, now we have to check the phase B and C + + // check the phase B + // find the highest magnitude in c_b + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)}; + max_i = -1; // max index + max_c = 0; // max current + max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cb[i]) continue; // current not measured + if(cb[i] > max_c){ + max_c = cb[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cb[j]) continue; // current not measured + float ratio = max_c / cb[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >= 1.5f){ + switch (max_i){ + case 0: // phase A is the max current + // this is an error as phase A is already aligned + SIMPLEFOC_DEBUG("CS: Err align B"); + return 0; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_b.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!"); + return 0; + }else{ //phase B is not measured so put the _NC to the phase B + if(_isset(pinB) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch B-(C)NC"); _swap(pinB, pinC); _swap(offset_ib, offset_ic); _swap(gain_b, gain_c); - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Error read B"); - // error in current sense - phase either not measured or bad connection - return 0; - } + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + } } - - // if phase C measured + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + // + // In either case A and B is done, now we have to check the phase C + // phase C is also aligned if it is measured (not _NC) + // we have to check if the current is negative and invert the gain if so if(_isset(pinC)){ - // set phase C active and phases A and B down - bldc_driver->setPwm(0, 0, voltage); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-A"); - // switch phase A and C - _swap(pinC, pinA); - _swap(offset_ic, offset_ia); - _swap(gain_c, gain_a); - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-B"); - _swap(pinC, pinB); - _swap(offset_ic, offset_ib); - _swap(gain_c, gain_b); - gain_b *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read C"); - // error in current sense - phase either not measured or bad connection - return 0; - } + if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity) + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } } - // add 2 if pin gains negative - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + + // construct the return flag + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; return exit_flag; } @@ -303,7 +394,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(0, 0); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -321,7 +412,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Neg A"); + SIMPLEFOC_DEBUG("CS: Inv A"); gain_a *= -1; } } @@ -339,7 +430,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // align phase A // check if measured current a is positive and invert if not if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Neg B"); + SIMPLEFOC_DEBUG("CS: Inv B"); gain_b *= -1; } } From 780bda930ed51520ece4882a5cfa1babfd60c228 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 11:47:34 +0200 Subject: [PATCH 207/220] exit flag currection for steppres --- src/common/base_classes/CurrentSense.cpp | 38 +++++++++++++----------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index e354dfaf..1b3ae498 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -187,19 +187,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // if yes throw an error and return 0 // either the current sense is not connected or the current is // too low for calibration purposes (one should raise the motor.voltage_sensor_align) - if((_isset(pinA) && fabs(c_a.a) < 0.1f) - || (_isset(pinB) && fabs(c_a.b) < 0.1f) - || (_isset(pinC) && fabs(c_a.c) < 0.1f)){ + if((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); return 0; // measurement current too low } - // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); - _delay(500); - PhaseCurrent_s c_b = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - // now we have to determine // 1) which pin correspond to which phase of the bldc driver // 2) if the currents measured have good polarity @@ -244,7 +236,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); _swap(c_a.b, c_a.b); - _swap(c_b.a, c_b.b); // for the next phase of alignment phases_switched = true; // signal that pins have been switched break; case 2: // phase C is the max current @@ -254,7 +245,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); _swap(c_a.a, c_a.c); - _swap(c_b.a, c_b.c); // for the next phase of alignment phases_switched = true;// signal that pins have been switched break; } @@ -276,7 +266,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); _swap(c_a.b, c_a.b); - _swap(c_b.a, c_b.b); // for the next phase of alignment phases_switched = true; // signal that pins have been switched }else if(_isset(pinA) && !_isset(pinC)){ SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); @@ -284,7 +273,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); _swap(c_a.b, c_a.c); - _swap(c_b.a, c_b.c); // for the next phase of alignment phases_switched = true; // signal that pins have been switched } } @@ -294,6 +282,13 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // // In either case A is done, now we have to check the phase B and C + + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + // check the phase B // find the highest magnitude in c_b // and make sure it's around 2 (1.5 at least) times higher than the other two @@ -386,7 +381,8 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ - int exit_flag = 1; + bool phases_switched = 0; + bool phases_inverted = 0; if(_isset(pinA)){ // set phase A active to high and B to low @@ -410,10 +406,11 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched + phases_switched = true; // signal that pins have been switched }else if (c.a < 0){ SIMPLEFOC_DEBUG("CS: Inv A"); gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted } } @@ -432,11 +429,18 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive if (c.b < 0){ SIMPLEFOC_DEBUG("CS: Inv B"); gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted } } - // add 2 if pin gains negative - if(gain_a < 0 || gain_b < 0 ) exit_flag +=2; + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; return exit_flag; } From ad0c4c526bad3c39ec5fc4e099d3ec3912fa648c Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:19:34 +0200 Subject: [PATCH 208/220] feactoring and added ramping for current sense align --- README.md | 3 +- src/common/base_classes/CurrentSense.cpp | 121 ++++++++++++++--------- src/common/foc_utils.h | 2 +- 3 files changed, 76 insertions(+), 50 deletions(-) diff --git a/README.md b/README.md index 9fb24dd1..543e6533 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 1b3ae498..5ef906e0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -177,11 +177,17 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ bool phases_switched = 0; bool phases_inverted = 0; - + float center = driver->voltage_limit/2.0; + // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -191,6 +197,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); return 0; // measurement current too low } + // now we have to determine // 1) which pin correspond to which phase of the bldc driver @@ -284,7 +291,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); bldc_driver->setPwm(0, 0, 0); @@ -384,53 +395,67 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive bool phases_switched = 0; bool phases_inverted = 0; - if(_isset(pinA)){ - // set phase A active to high and B to low - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - // disable the phases - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - // check if current b is around zero and if its not - // check if current a is near zero and if it is invert them - if (fabs(c.a) < fabs(c.b)){ - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B - _swap(pinA, pinB); - _swap(offset_ia, offset_ib); - _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - phases_switched = true; // signal that pins have been switched - }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Inv A"); - gain_a *= -1; - phases_inverted = true; // signal that pins have been inverted - } + if(!_isset(pinA) || !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); + return 0; } - if(_isset(pinB)){ - // set phase B active and phases A and C down - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Inv B"); - gain_b *= -1; - phases_inverted = true; // signal that pins have been inverted - } + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // 1) only one phase can be measured so we first measure which ADC pin corresponds + // to the phase A by comparing the magnitude + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + phases_switched = true; // signal that pins have been switched + } + // 2) check if measured current a is positive and invert if not + if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(0, voltage/100.0*((float)i)); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + + // phase B should be aligned + // 1) we just need to verify that it has been measured + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // 2) check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted } // construct the return flag diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 9a5b53ea..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -35,7 +35,7 @@ #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -#define _NC (NOT_SET) +#define _NC ((int) NOT_SET) #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f) From 9aee1aa6a10efe3526b2c3e85273fe372feafc01 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:21:20 +0200 Subject: [PATCH 209/220] removed accidentally added files --- src/BLDCMotor.cpp.old | 732 ------------------------------------------ src/BLDCMotor.h.old | 121 ------- src/SimpleFOC_empty.h | 119 ------- 3 files changed, 972 deletions(-) delete mode 100644 src/BLDCMotor.cpp.old delete mode 100644 src/BLDCMotor.h.old delete mode 100644 src/SimpleFOC_empty.h diff --git a/src/BLDCMotor.cpp.old b/src/BLDCMotor.cpp.old deleted file mode 100644 index 6b996342..00000000 --- a/src/BLDCMotor.cpp.old +++ /dev/null @@ -1,732 +0,0 @@ -#include "BLDCMotor.h" -#include "./communication/SimpleFOCDebug.h" - -// BLDCMotor( int pp , float R) -// - pp - pole pair number -// - R - motor phase resistance -// - KV - motor kv rating -BLDCMotor::BLDCMotor(int pp, float _R, float _KV) -: FOCMotor() -{ - // save pole pairs number - pole_pairs = pp; - // save phase resistance number - phase_resistance = _R; - // save back emf constant KV = 1/KV - K_bemf = _isset(_KV) ? 1.0/_KV : NOT_SET; - - // torque control type is voltage by default - torque_controller = TorqueControlType::voltage; -} - - -/** - Link the driver which controls the motor -*/ -void BLDCMotor::linkDriver(BLDCDriver* _driver) { - driver = _driver; -} - -// init hardware pins -void BLDCMotor::init() { - if (!driver || !driver->initialized) { - motor_status = FOCMotorStatus::motor_init_failed; - SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; - } - motor_status = FOCMotorStatus::motor_initializing; - SIMPLEFOC_DEBUG("MOT: Init"); - - // sanity check for the voltage limit configuration - if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; - // constrain voltage for sensor alignment - if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - - // update the controller limits - if(current_sense){ - // current control loop controls voltage - PID_current_q.limit = voltage_limit; - PID_current_d.limit = voltage_limit; - } - if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){ - // velocity control loop controls current - PID_velocity.limit = current_limit; - }else{ - // velocity control loop controls the voltage - PID_velocity.limit = voltage_limit; - } - P_angle.limit = velocity_limit; - - _delay(500); - // enable motor - SIMPLEFOC_DEBUG("MOT: Enable driver."); - enable(); - _delay(500); - motor_status = FOCMotorStatus::motor_uncalibrated; -} - - -// disable motor driver -void BLDCMotor::disable() -{ - // set zero to PWM - driver->setPwm(0, 0, 0); - // disable the driver - driver->disable(); - // motor status update - enabled = 0; -} -// enable motor driver -void BLDCMotor::enable() -{ - // enable the driver - driver->enable(); - // set zero to PWM - driver->setPwm(0, 0, 0); - // motor status update - enabled = 1; -} - -/** - FOC functions -*/ -// FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { - int exit_flag = 1; - - motor_status = FOCMotorStatus::motor_calibrating; - - // align motor if necessary - // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - - // sensor and motor alignment - can be skipped - // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); - if(sensor){ - exit_flag *= alignSensor(); - // added the shaft_angle update - sensor->update(); - shaft_angle = shaftAngle(); - }else - SIMPLEFOC_DEBUG("MOT: No sensor."); - - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - _delay(500); - if(exit_flag){ - if(current_sense) exit_flag *= alignCurrentSense(); - else SIMPLEFOC_DEBUG("MOT: No current sense."); - } - - if(exit_flag){ - SIMPLEFOC_DEBUG("MOT: Ready."); - motor_status = FOCMotorStatus::motor_ready; - }else{ - SIMPLEFOC_DEBUG("MOT: Init FOC failed."); - motor_status = FOCMotorStatus::motor_calib_failed; - disable(); - } - - return exit_flag; -} - -// Calibarthe the motor and current sense phases -int BLDCMotor::alignCurrentSense() { - int exit_flag = 1; // success - - SIMPLEFOC_DEBUG("MOT: Align current sense."); - - // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); - if(!exit_flag){ - // error in current sense - phase either not measured or bad connection - SIMPLEFOC_DEBUG("MOT: Align error!"); - exit_flag = 0; - }else{ - // output the alignment status flag - SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); - } - - return exit_flag > 0; -} - -// Encoder alignment to electrical 0 angle -int BLDCMotor::alignSensor() { - int exit_flag = 1; //success - SIMPLEFOC_DEBUG("MOT: Align sensor."); - - // check if sensor needs zero search - if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); - // stop init if not found index - if(!exit_flag) return exit_flag; - - // if unknown natural direction - if(!_isset(sensor_direction)){ - - // find natural direction - // move one electrical revolution forward - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // take and angle in the middle - sensor->update(); - float mid_angle = sensor->getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - sensor->update(); - float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); - _delay(200); - // determine the direction the sensor moved - if (mid_angle == end_angle) { - SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); - return 0; // failed calibration - } else if (mid_angle < end_angle) { - SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); - sensor_direction = Direction::CCW; - } else{ - SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); - sensor_direction = Direction::CW; - } - // check pole pair number - float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else - SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - - } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); - - // zero electric angle not known - if(!_isset(zero_electric_angle)){ - - // // align the electrical phases of the motor and sensor - // // set angle -90(270 = 3PI/2) degrees - // setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - // _delay(700); - // // read the sensor - // sensor->update(); - // // get the current zero electric angle - // zero_electric_angle = 0; - // zero_electric_angle = electricalAngle(); - // _delay(20); - // if(monitor_port){ - // SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); - // } - // // stop everything - // setPhaseVoltage(0, 0, 0); - // _delay(200); - - - // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - _delay(700); - // read the sensor - sensor->update(); - - float d_angle = pole_pairs*_2PI/((float) calib_points); - for(int i=0; i< calib_points; i++){ - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2+d_angle*i); - _delay(50); - // read the sensor - sensor->update(); - // get the current zero electric angle - float sensor_angle = (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle(); - zero_electric_angle_array[i] = _normalizeAngle( sensor_angle - d_angle*i); - // current_angle_offset_array[i] = 0; - // for(int n=0; n<20;n++){ - // DQCurrent_s c = current_sense->getFOCCurrents(0); // get alpha(d) and beta(q) currents - // current_angle_offset_array[i] += sensor_angle - atan2(c.q, c.d); - // _delay(1); - // } - // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i]/20.0f ); - // if(i>=1){ - // float d_an = current_angle_offset_array[i] - current_angle_offset_array[i-1]; - // if( fabs(d_an) > 0.05){ - // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i-1]+_sign(d_an)*0.05); - // } - // } - if(monitor_port){ - SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle_array[i]); - // SimpleFOCDebug::print(_normalizeAngle(zero_electric_angle_array[i]+_PI_2)-_PI_2); - // SimpleFOCDebug::print("\t"); - // SimpleFOCDebug::println(_normalizeAngle(current_angle_offset_array[i]+_PI_2)-_PI_2); - } - } - // stop everything - setPhaseVoltage(0, 0, 0); - // _delay(200); - - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); - return exit_flag; -} - -float BLDCMotor::electricalAngle(){ - // if no sensor linked return previous value ( for open loop ) - if(!sensor) return electrical_angle; - float angle = sensor->getMechanicalAngle(); - zero_electric_angle = zero_electric_angle_array[_round(angle/_2PI*calib_points) % calib_points ]; - return _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - zero_electric_angle ); -} - -// Encoder alignment the absolute zero angle -// - to the index -int BLDCMotor::absoluteZeroSearch() { - // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision - // of float is sufficient. - SIMPLEFOC_DEBUG("MOT: Index search..."); - // search the absolute zero with small velocity - float limit_vel = velocity_limit; - float limit_volt = voltage_limit; - velocity_limit = velocity_index_search; - voltage_limit = voltage_sensor_align; - shaft_angle = 0; - while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5f*_2PI); - // call important for some sensors not to loose count - // not needed for the search - sensor->update(); - } - // disable motor - setPhaseVoltage(0, 0, 0); - // reinit the limits - velocity_limit = limit_vel; - voltage_limit = limit_volt; - // check if the zero found - if(monitor_port){ - if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); - } - return !sensor->needsSearch(); -} - -// Iterative function looping FOC algorithm, setting Uq on the Motor -// The faster it can be run the better -void BLDCMotor::loopFOC() { - // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track - // of full rotations otherwise. - if (sensor) sensor->update(); - - // if open-loop do nothing - if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - - // if disabled do nothing - if(!enabled) return; - - // Needs the update() to be called first - // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() - // which is in range 0-2PI - electrical_angle = electricalAngle(); - float current_electrical_angle = electrical_angle; - // float angle = sensor->getMechanicalAngle(); - // float current_angle_offset = current_angle_offset_array[_round(angle/_2PI*calib_points) % calib_points ]; - // float current_electrical_angle = _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - current_angle_offset ); - switch (torque_controller) { - - case TorqueControlType::voltage: - // no need to do anything really - break; - case TorqueControlType::dc_current: - if(!current_sense) return; - // read overall current magnitude - current.q = current_sense->getDCCurrent(current_electrical_angle); - // filter the value values - current.q = LPF_current_q(current.q); - // calculate the phase voltage - voltage.q = PID_current_q(current_sp - current.q); - voltage.d = 0; - break; - case TorqueControlType::foc_current: - if(!current_sense) return; - // read dq currents - current = current_sense->getFOCCurrents(current_electrical_angle); - // filter values - current.q = LPF_current_q(current.q); - current.d = LPF_current_d(current.d); - // calculate the phase voltages - voltage.q = PID_current_q(current_sp - current.q); - voltage.d = PID_current_d(-current.d); - break; - default: - // no torque control selected - SIMPLEFOC_DEBUG("MOT: no torque control selected!"); - break; - } - - // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage.q, voltage.d, electrical_angle); -} - -// Iterative function running outer loop of the FOC algorithm -// Behavior of this function is determined by the motor.controller variable -// It runs either angle, velocity or torque loop -// - needs to be called iteratively it is asynchronous function -// - if target is not set it uses motor.target value -void BLDCMotor::move(float new_target) { - - // downsampling (optional) - if(motion_cnt++ < motion_downsample) return; - motion_cnt = 0; - - // shaft angle/velocity need the update() to be called first - // get shaft angle - // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float - // For this reason it is NOT precise when the angles become large. - // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem - // when switching to a 2-component representation. - if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) - shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode - // get angular velocity - shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated - - // if disabled do nothing - if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; - - // calculate the back-emf voltage if K_bemf available - if (_isset(K_bemf)) voltage_bemf = K_bemf*shaft_velocity; - // estimate the motor current if phase reistance available and current_sense not available - if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; - - // upgrade the current based voltage limit - switch (controller) { - case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage){ // if voltage torque control - if(!_isset(phase_resistance)) voltage.q = target; - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - voltage.d = 0; - }else{ - current_sp = target; // if current/foc_current torque control - } - break; - case MotionControlType::angle: - // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when - // the angles are large. This results in not being able to command small changes at high position values. - // to solve this, the delta-angle has to be calculated in a numerically precise way. - // angle set point - shaft_angle_sp = target; - // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage - if(torque_controller == TorqueControlType::voltage){ - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - voltage.d = 0; - } - break; - case MotionControlType::velocity: - // velocity set point - sensor precision: this calculation is numerically precise. - shaft_velocity_sp = target; - // calculate the torque command - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control - if(torque_controller == TorqueControlType::voltage){ - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - voltage.d = 0; - } - break; - case MotionControlType::velocity_openloop: - // velocity control in open loop - sensor precision: this calculation is numerically precise. - shaft_velocity_sp = target; - voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor - voltage.d = 0; - break; - case MotionControlType::angle_openloop: - // angle control in open loop - - // TODO sensor precision: this calculation NOT numerically precise, and subject - // to the same problems in small set-point changes at high angles - // as the closed loop version. - shaft_angle_sp = target; - voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor - voltage.d = 0; - break; - } -} - - -// Method using FOC to set Uq and Ud to the motor at the optimal angle -// Function implementing Space Vector PWM and Sine PWM algorithms -// -// Function using sine approximation -// regular sin + cos ~300us (no memory usaage) -// approx _sin + _cos ~110us (400Byte ~ 20% of memory) -void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - - float center; - int sector; - float _ca,_sa; - - switch (foc_modulation) - { - case FOCModulationType::Trapezoid_120 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 - static int trap_120_map[6][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_120_state = 0; - sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - - if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){ - Ua= center; - Ub = trap_120_map[sector][1] * Uq + center; - Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible - }else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){ - Ua = trap_120_map[sector][0] * Uq + center; - Ub = center; - Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ - Ua = trap_120_map[sector][0] * Uq + center; - Ub = trap_120_map[sector][1] * Uq + center; - Uc = center; - driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible - } - - break; - - case FOCModulationType::Trapezoid_150 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 - static int trap_150_map[12][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_150_state = 0; - sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - - if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){ - Ua= center; - Ub = trap_150_map[sector][1] * Uq + center; - Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible - }else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){ - Ua = trap_150_map[sector][0] * Uq + center; - Ub = center; - Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ - Ua = trap_150_map[sector][0] * Uq + center; - Ub = trap_150_map[sector][1] * Uq + center; - Uc = center; - driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible - } - - break; - - case FOCModulationType::SinePWM : - // Sinusoidal PWM modulation - // Inverse Park + Clarke transformation - - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); - // Inverse park transform - Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; - Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; - - // center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - center = driver->voltage_limit/2; - // Clarke transform - Ua = Ualpha + center; - Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; - - if (!modulation_centered) { - float Umin = min(Ua, min(Ub, Uc)); - Ua -= Umin; - Ub -= Umin; - Uc -= Umin; - } - - break; - - case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm - // https://www.youtube.com/watch?v=QMSWUMEAejg - - // the algorithm goes - // 1) Ualpha, Ubeta - // 2) Uout = sqrt(Ualpha^2 + Ubeta^2) - // 3) angle_el = atan2(Ubeta, Ualpha) - // - // equivalent to 2) because the magnitude does not change is: - // Uout = sqrt(Ud^2 + Uq^2) - // equivalent to 3) is - // angle_el = angle_el + atan2(Uq,Ud) - - float Uout; - // a bit of optitmisation - if(Ud){ // only if Ud and Uq set - // _sqrt is an approx of sqrt (3-4% error) - Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); - }else{// only Uq available - no need for atan2 and sqrt - Uout = Uq / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + _PI_2); - } - // find the sector we are in currently - sector = floor(angle_el / _PI_3) + 1; - // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; - // two versions possible - float T0 = 0; // pulled to 0 - better for low power supply voltage - if (modulation_centered) { - T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 - } - - // calculate the duty cycles(times) - float Ta,Tb,Tc; - switch(sector){ - case 1: - Ta = T1 + T2 + T0/2; - Tb = T2 + T0/2; - Tc = T0/2; - break; - case 2: - Ta = T1 + T0/2; - Tb = T1 + T2 + T0/2; - Tc = T0/2; - break; - case 3: - Ta = T0/2; - Tb = T1 + T2 + T0/2; - Tc = T2 + T0/2; - break; - case 4: - Ta = T0/2; - Tb = T1+ T0/2; - Tc = T1 + T2 + T0/2; - break; - case 5: - Ta = T2 + T0/2; - Tb = T0/2; - Tc = T1 + T2 + T0/2; - break; - case 6: - Ta = T1 + T2 + T0/2; - Tb = T0/2; - Tc = T1 + T0/2; - break; - default: - // possible error state - Ta = 0; - Tb = 0; - Tc = 0; - } - - // calculate the phase voltages and center - Ua = Ta*driver->voltage_limit; - Ub = Tb*driver->voltage_limit; - Uc = Tc*driver->voltage_limit; - break; - - } - - // set the voltages in driver - driver->setPwm(Ua, Ub, Uc); -} - - - -// Function (iterative) generating open loop movement for target velocity -// - target_velocity - rad/s -// it uses voltage_limit variable -float BLDCMotor::velocityOpenloop(float target_velocity){ - // get current timestamp - unsigned long now_us = _micros(); - // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6f; - // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - - // calculate the necessary angle to achieve target velocity - shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); - // for display purposes - shaft_velocity = target_velocity; - - // use voltage limit or current limit - float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); - - // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); - - // save timestamp for next call - open_loop_timestamp = now_us; - - return Uq; -} - -// Function (iterative) generating open loop movement towards the target angle -// - target_angle - rad -// it uses voltage_limit and velocity_limit variables -float BLDCMotor::angleOpenloop(float target_angle){ - // get current timestamp - unsigned long now_us = _micros(); - // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6f; - // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - - // calculate the necessary angle to move from current position towards target angle - // with maximal velocity (velocity_limit) - // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point - // where small position changes are no longer captured by the precision of floats - // when the total position is large. - if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; - shaft_velocity = velocity_limit; - }else{ - shaft_angle = target_angle; - shaft_velocity = 0; - } - - // use voltage limit or current limit - float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); - // set the maximal allowed voltage (voltage_limit) with the necessary angle - // sensor precision: this calculation is OK due to the normalisation - setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); - - // save timestamp for next call - open_loop_timestamp = now_us; - - return Uq; -} diff --git a/src/BLDCMotor.h.old b/src/BLDCMotor.h.old deleted file mode 100644 index 04c54b92..00000000 --- a/src/BLDCMotor.h.old +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef BLDCMotor_h -#define BLDCMotor_h - -#include "Arduino.h" -#include "common/base_classes/FOCMotor.h" -#include "common/base_classes/Sensor.h" -#include "common/base_classes/BLDCDriver.h" -#include "common/foc_utils.h" -#include "common/time_utils.h" -#include "common/defaults.h" - -/** - BLDC motor class -*/ -class BLDCMotor: public FOCMotor -{ - public: - /** - BLDCMotor class constructor - @param pp pole pairs number - @param R motor phase resistance - @param KV motor KV rating (1/K_bemf) - */ - BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET); - - /** - * Function linking a motor and a foc driver - * - * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting - */ - void linkDriver(BLDCDriver* driver); - - /** - * BLDCDriver link: - * - 3PWM - * - 6PWM - */ - BLDCDriver* driver; - - /** Motor hardware init function */ - void init() override; - /** Motor disable function */ - void disable() override; - /** Motor enable function */ - void enable() override; - - /** - * Function initializing FOC algorithm - * and aligning sensor's and motors' zero position - */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; - /** - * Function running FOC algorithm in real-time - * it calculates the gets motor angle and sets the appropriate voltages - * to the phase pwm signals - * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us - */ - void loopFOC() override; - - /** - * Function executing the control loops set by the controller parameter of the BLDCMotor. - * - * @param target Either voltage, angle or velocity based on the motor.controller - * If it is not set the motor will use the target set in its variable motor.target - * - * This function doesn't need to be run upon each loop execution - depends of the use case - */ - void move(float target = NOT_SET) override; - - float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - - - - float electricalAngle(); - int calib_points = 1000; - float zero_electric_angle_array[1000]; - float current_angle_offset_array[1000]; - - private: - // FOC methods - /** - * Method using FOC to set Uq to the motor at the optimal angle - * Heart of the FOC algorithm - * - * @param Uq Current voltage in q axis to set to the motor - * @param Ud Current voltage in d axis to set to the motor - * @param angle_el current electrical angle of the motor - */ - void setPhaseVoltage(float Uq, float Ud, float angle_el); - /** Sensor alignment to electrical 0 angle of the motor */ - int alignSensor(); - /** Current sense and motor phase alignment */ - int alignCurrentSense(); - /** Motor and sensor alignment to the sensors absolute 0 angle */ - int absoluteZeroSearch(); - - - // Open loop motion control - /** - * Function (iterative) generating open loop movement for target velocity - * it uses voltage_limit variable - * - * @param target_velocity - rad/s - */ - float velocityOpenloop(float target_velocity); - /** - * Function (iterative) generating open loop movement towards the target angle - * it uses voltage_limit and velocity_limit variables - * - * @param target_angle - rad - */ - float angleOpenloop(float target_angle); - // open loop variables - long open_loop_timestamp; - - -}; - - -#endif diff --git a/src/SimpleFOC_empty.h b/src/SimpleFOC_empty.h deleted file mode 100644 index bf0049dd..00000000 --- a/src/SimpleFOC_empty.h +++ /dev/null @@ -1,119 +0,0 @@ -/*! - * @file SimpleFOC.h - * - * @mainpage Simple Field Oriented Control BLDC motor control library - * - * @section intro_sec Introduction - * - * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.
Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to: - * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library - * - Develop a modular BLDC driver board: Arduino SimpleFOC shield. - * - * @section features Features - * - Arduino compatible: Arduino library code - * - Easy to setup and configure: - * - Easy hardware configuration - * - Easy tuning the control loops - * - Modular: - * - Supports as many sensors , BLDC motors and driver boards as possible - * - Supports as many application requirements as possible - * - Plug & play: Arduino SimpleFOC shield - * - * @section dependencies Supported Hardware - * - Motors - * - BLDC motors - * - Stepper motors - * - Drivers - * - BLDC drivers - * - Gimbal drivers - * - Stepper drivers - * - Position sensors - * - Encoders - * - Magnetic sensors - * - Hall sensors - * - Open-loop control - * - Microcontrollers - * - Arduino - * - STM32 - * - ESP32 - * - Teensy - * - * @section example_code Example code - * @code -#include - -// BLDCMotor( pole_pairs ) -BLDCMotor motor = BLDCMotor(11); -// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) -BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); -// Encoder(pin_A, pin_B, CPR) -Encoder encoder = Encoder(2, 3, 2048); -// channel A and B callbacks -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - - -void setup() { - // initialize encoder hardware - encoder.init(); - // hardware interrupt enable - encoder.enableInterrupts(doA, doB); - // link the motor to the sensor - motor.linkSensor(&encoder); - - // power supply voltage [V] - driver.voltage_power_supply = 12; - // initialise driver hardware - driver.init(); - // link driver - motor.linkDriver(&driver); - - // set control loop type to be used - motor.controller = MotionControlType::velocity; - // initialize motor - motor.init(); - - // align encoder and start FOC - motor.initFOC(); -} - -void loop() { - // FOC algorithm function - motor.loopFOC(); - - // velocity control loop function - // setting the target velocity or 2rad/s - motor.move(2); -} - * @endcode - * - * @section license License - * - * MIT license, all text here must be included in any redistribution. - * - */ - -#ifndef SIMPLEFOC_EMPTY_H -#define SIMPLEFOC_EMPTY_H - -// #include "BLDCMotor.h" -// #include "StepperMotor.h" -// #include "sensors/Encoder.h" -// #include "sensors/MagneticSensorSPI.h" -// #include "sensors/MagneticSensorI2C.h" -// #include "sensors/MagneticSensorAnalog.h" -// #include "sensors/MagneticSensorPWM.h" -// #include "sensors/HallSensor.h" -// #include "sensors/GenericSensor.h" -// #include "drivers/BLDCDriver3PWM.h" -// #include "drivers/BLDCDriver6PWM.h" -// #include "drivers/StepperDriver4PWM.h" -// #include "drivers/StepperDriver2PWM.h" -// #include "current_sense/InlineCurrentSense.h" -// #include "current_sense/LowsideCurrentSense.h" -// #include "current_sense/GenericCurrentSense.h" -// #include "communication/Commander.h" -// #include "communication/StepDirListener.h" -// #include "communication/SimpleFOCDebug.h" - -#endif From cf70cce014fc94f290c9df9315a8ee23dc22e543 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 14:01:55 +0200 Subject: [PATCH 210/220] added the contering posibility --- src/BLDCMotor.cpp | 2 +- src/StepperMotor.cpp | 2 +- src/common/base_classes/CurrentSense.cpp | 32 +++++++++++++---------- src/common/base_classes/CurrentSense.h | 6 ++--- src/current_sense/GenericCurrentSense.cpp | 2 +- src/current_sense/GenericCurrentSense.h | 2 +- 6 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index acc5b0fe..792381d5 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 38e8999f..5db9afa2 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -159,7 +159,7 @@ int StepperMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 5ef906e0..84bc3547 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -133,7 +133,7 @@ void CurrentSense::disable(){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted // IMPORTANT, this function can be overriden in the child class -int CurrentSense::driverAlign(float voltage){ +int CurrentSense::driverAlign(float voltage, bool modulation_centered){ int exit_flag = 1; if(skip_align) return exit_flag; @@ -142,9 +142,9 @@ int CurrentSense::driverAlign(float voltage){ // check if stepper or BLDC if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver); + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); else - return alignBLDCDriver(voltage, (BLDCDriver*)driver); + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); } @@ -172,22 +172,23 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; - float center = driver->voltage_limit/2.0; + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; // set phase A active and phases B and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -293,12 +294,12 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check the phase B // find the highest magnitude in c_b @@ -390,10 +391,13 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; + + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; if(!_isset(pinA) || !_isset(pinB)){ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); @@ -403,13 +407,13 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase A active and phases B down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(voltage/100.0*((float)i), 0); + stepper_driver->setPwm(voltage/100.0*((float)i), zero); _delay(3); } _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -438,12 +442,12 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase B active and phases A down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(0, voltage/100.0*((float)i)); + stepper_driver->setPwm(zero, voltage/100.0*((float)i)+zero); _delay(3); } _delay(500); c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); // phase B should be aligned // 1) we just need to verify that it has been measured diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index c9c79650..d3f7f8ed 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -68,7 +68,7 @@ class CurrentSense{ * * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage); + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -130,11 +130,11 @@ class CurrentSense{ /** * Function used to align the current sense with the BLDC motor driver */ - int alignBLDCDriver(float align_voltage, BLDCDriver* driver); + int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); /** * Function used to align the current sense with the Stepper motor driver */ - int alignStepperDriver(float align_voltage, StepperDriver* driver); + int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); /** * Function used to read the average current values over N samples */ diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 9d90f0ca..54c4f12e 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,7 +54,7 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -int GenericCurrentSense::driverAlign(float voltage){ +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index a63df49d..02bf8fa9 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; + int driverAlign(float align_voltage, bool modulation_centered) override; PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading From dbc62a1d5e913af6436d8c36eb57cda3cebbb152 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 14:03:51 +0200 Subject: [PATCH 211/220] a correction for stepper --- src/common/base_classes/CurrentSense.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 84bc3547..a3966ad6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -392,12 +392,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ - + + _UNUSED(modulation_centered); + bool phases_switched = 0; bool phases_inverted = 0; - - float zero = 0; - if(modulation_centered) zero = driver->voltage_limit/2.0; if(!_isset(pinA) || !_isset(pinB)){ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); @@ -407,13 +406,13 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase A active and phases B down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(voltage/100.0*((float)i), zero); + stepper_driver->setPwm(voltage/100.0*((float)i), 0); _delay(3); } _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(zero, zero); + stepper_driver->setPwm(0, 0); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -442,12 +441,12 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase B active and phases A down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(zero, voltage/100.0*((float)i)+zero); + stepper_driver->setPwm(0, voltage/100.0*((float)i)); _delay(3); } _delay(500); c = readAverageCurrents(); - stepper_driver->setPwm(zero, zero); + stepper_driver->setPwm(0, 0); // phase B should be aligned // 1) we just need to verify that it has been measured From bd21d2b1ecd66293e4982e9f4c542f4b3ddc7f6f Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sat, 20 Jul 2024 16:06:26 +0200 Subject: [PATCH 212/220] Update CurrentSense.cpp --- src/common/base_classes/CurrentSense.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index a3966ad6..4813dc3b 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -450,8 +450,8 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // phase B should be aligned // 1) we just need to verify that it has been measured - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); + if (fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current on B!"); return 0; // measurement current too low } // 2) check if measured current a is positive and invert if not From d429148f85c775a9fe43c03f8f0394024efc118c Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:08:23 +0200 Subject: [PATCH 213/220] refactored examples --- .../single_full_control_example.ino | 2 +- .../open_loop_velocity_example.ino | 11 ++++- .../inline_current_sense_test.ino | 5 +- .../bldc_driver_3pwm_standalone.ino | 7 ++- .../bldc_driver_6pwm_standalone.ino | 7 ++- .../stepper_driver_2pwm_standalone.ino | 7 ++- .../stepper_driver_4pwm_standalone.ino | 7 ++- .../hall_sensor_example.ino | 9 ---- ...all_sensor_hardware_interrupts_example.ino | 48 +++++++++++++++++++ 9 files changed, 82 insertions(+), 21 deletions(-) create mode 100644 examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 9c80a36f..6359f201 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -13,7 +13,7 @@ void doB(){encoder.handleB();} // inline current sensor instance // ACS712-05B has the resolution of 0.185mV per Amp -InlineCurrentSense current_sense = InlineCurrentSense(1.0f, 0.185f, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); // commander communication instance Commander command = Commander(Serial); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 075dc9c6..546ac3ea 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -36,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -50,7 +53,11 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } + // add target command T command.add('T', doTarget, "target velocity"); diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 195021eb..1198cdcd 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -19,7 +19,10 @@ void setup() { SimpleFOCDebug::enable(&Serial); // initialise the current sensing - current_sense.init(); + if(!current_sense.init()){ + Serial.println("Current sense init failed."); + return; + } // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; diff --git a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 82716353..eef793d7 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -23,11 +23,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 1ed37441..56a1afbe 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -24,11 +24,14 @@ void setup() { driver.dead_zone = 0.05f; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 889143e2..59343a14 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -29,11 +29,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index 95dcc589..a58d7940 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -24,11 +24,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index c4777baa..cc8dfdb8 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -13,13 +13,6 @@ // - pp - pole pairs HallSensor sensor = HallSensor(2, 3, 4, 14); -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){sensor.handleA();} -void doB(){sensor.handleB();} -void doC(){sensor.handleC();} - - void setup() { // monitoring port Serial.begin(115200); @@ -29,8 +22,6 @@ void setup() { // initialise encoder hardware sensor.init(); - // hardware interrupt enable - sensor.enableInterrupts(doA, doB, doC); Serial.println("Sensor ready"); _delay(1000); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino new file mode 100644 index 00000000..c4777baa --- /dev/null +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino @@ -0,0 +1,48 @@ +/** + * Hall sensor example code + * + * This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup. + * + */ + +#include + +// Hall sensor instance +// HallSensor(int hallA, int hallB , int cpr, int index) +// - hallA, hallB, hallC - HallSensor A, B and C pins +// - pp - pole pairs +HallSensor sensor = HallSensor(2, 3, 4, 14); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} + + +void setup() { + // monitoring port + Serial.begin(115200); + + // check if you need internal pullups + sensor.pullup = Pullup::USE_EXTERN; + + // initialise encoder hardware + sensor.init(); + // hardware interrupt enable + sensor.enableInterrupts(doA, doB, doC); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); + delay(100); +} From b2647a63cfd33b864602f31ffead17fab9954171 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:09:32 +0200 Subject: [PATCH 214/220] error in timer align (forgotten ifdef --- .../hardware_specific/stm32/stm32_mcu.cpp | 53 +++++++++++++++++-- 1 file changed, 48 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 0fdec2f0..4009281e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -249,7 +249,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (TIM_master == TIM1){ - if(TIM_slave == TIM2 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; #endif @@ -257,7 +265,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM2) && defined(LL_TIM_TS_ITR1) else if (TIM_master == TIM2){ - if(TIM_slave == TIM1 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; #endif @@ -268,7 +284,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM3) && defined(LL_TIM_TS_ITR2) else if (TIM_master == TIM3){ - if(TIM_slave== TIM1 || TIM_slave == TIM2 || TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #endif #if defined(TIM5) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif @@ -276,7 +300,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM4) && defined(LL_TIM_TS_ITR3) else if (TIM_master == TIM4){ - if(TIM_slave == TIM1 || TIM_slave == TIM2 || TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; #endif @@ -288,9 +320,13 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #if defined(TIM5) else if (TIM_master == TIM5){ #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + #if defined(TIM1) if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; #endif + #endif #if defined(TIM8) if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; #endif @@ -298,8 +334,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM8) else if (TIM_master == TIM8){ + #if defined(TIM2) if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; - else if(TIM_slave ==TIM4 || TIM_slave ==TIM5) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif } #endif return -1; // combination not supported From 150a40b284369ce207f1d8838a3ad6198b604459 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:14:35 +0200 Subject: [PATCH 215/220] error in exampels --- .../open_loop_velocity_example.ino | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 546ac3ea..c439bf70 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -53,11 +53,7 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - if(!motor.init()){ - Serial.println("Motor init failed!"); - return; - } - + motor.init(); // add target command T command.add('T', doTarget, "target velocity"); From 05672d28187320c10f74f8fa382e92b09cfadce0 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:20:16 +0200 Subject: [PATCH 216/220] motor init returns success --- .../open_loop_position_example.ino | 10 ++++++++-- .../open_loop_velocity_example.ino | 5 ++++- src/BLDCMotor.cpp | 3 ++- src/BLDCMotor.h | 2 +- src/StepperMotor.cpp | 3 ++- src/StepperMotor.h | 2 +- src/common/base_classes/FOCMotor.h | 2 +- 7 files changed, 19 insertions(+), 8 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 6bcfdff9..c1ff7bbc 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -36,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -52,7 +55,10 @@ void setup() { motor.controller = MotionControlType::angle_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target angle"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index c439bf70..b1bc2760 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -53,7 +53,10 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target velocity"); diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 792381d5..c127949e 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -67,7 +67,7 @@ void BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -105,6 +105,7 @@ void BLDCMotor::init() { enable(); _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 41a5a1c0..c261e405 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -40,7 +40,7 @@ class BLDCMotor: public FOCMotor BLDCDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5db9afa2..7b4a5820 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -37,7 +37,7 @@ void StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -70,6 +70,7 @@ void StepperMotor::init() { _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index f76e7bcf..7e7810d8 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -43,7 +43,7 @@ class StepperMotor: public FOCMotor StepperDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index b5ba2e96..8ae0e8af 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -78,7 +78,7 @@ class FOCMotor FOCMotor(); /** Motor hardware init function */ - virtual void init()=0; + virtual int init()=0; /** Motor disable function */ virtual void disable()=0; /** Motor enable function */ From ea8eda83d265f3589a2d0ae6669373641daedf7b Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:25:33 +0200 Subject: [PATCH 217/220] typo in transition from void to int --- src/BLDCMotor.cpp | 2 +- src/StepperMotor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c127949e..501a8bc9 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -63,7 +63,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { } // init hardware pins -void BLDCMotor::init() { +int BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 7b4a5820..1d8f3147 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -33,7 +33,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { } // init hardware pins -void StepperMotor::init() { +int StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); From 5afea2b88e0569adb3cc2834c79a4c35280b4a35 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:26:18 +0200 Subject: [PATCH 218/220] preparations for v2.3.4 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 0a2606b2..5daa49d9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.3 +version=2.3.4 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 37ca90d673a997b72c58247f0aa1f4ca10f21c94 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 14:05:27 +0200 Subject: [PATCH 219/220] readme for v2.3.4 --- README.md | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 543e6533..4fbb7517 100644 --- a/README.md +++ b/README.md @@ -30,10 +30,26 @@ Therefore this is an attempt to: - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 -> - Current sensing support for Stepper motors (lowside and inline) - - -## Arduino *SimpleFOClibrary* v2.3.3 +> - ESP32 MCUs extended support [#414](https://github.com/simplefoc/Arduino-FOC/pull/414) +> - Transition to the arduino-esp32 version v3.x (ESP-IDF v5.x) [#387](https://github.com/espressif/arduino-esp32/releases) +> - New support for MCPWM driver +> - New support for LEDC drivers - center-aligned PWM and 6PWM available +> - Rewritten and simplified the fast ADC driver code (`adcRead`) - for low-side and inline current sensing. +> - Stepper motors current sensing support [#421](https://github.com/simplefoc/Arduino-FOC/pull/421) +> - Support for current sensing (low-side and inline) - [see in docs](https://docs.simplefoc.com/current_sense) +> - Support for true FOC control - `foc_current` torque control - [see in docs](https://docs.simplefoc.com/motion_control) +> - New current sense alignment procedure [#422](https://github.com/simplefoc/Arduino-FOC/pull/422) - [see in docs](https://docs.simplefoc.com/current_sense_align) +> - Support for steppers +> - Much more robust and reliable +> - More verbose and informative +> - Support for HallSensors without interrupts [#424](https://docs.simplefoc.com/https://github.com/simplefoc/Arduino-FOC/pull/424) - [see in docs](hall_sensors) +> - Docs +> - A short guide to debugging of common issues +> - A short guide to the units in the library - [see in docs](https://docs.simplefoc.com/library_units) +> - See the complete list of bugfixes and new features of v2.3.4 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/11) + + +## Arduino *SimpleFOClibrary* v2.3.4

From 97abb54cd90e6fcad126b9694e4c3e15cec5239a Mon Sep 17 00:00:00 2001 From: Rob Deutsch Date: Fri, 17 Jan 2025 23:20:46 +1100 Subject: [PATCH 220/220] Update esp32_ledc_mcu.cpp to ensure ledc_timer is initialised to a known state (ledc_timer = false) --- src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index dc667ab3..c5ba1c78 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -76,6 +76,7 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_timer_bit_t res = static_cast(resolution); ledc_timer_config_t ledc_timer; + memset(&ledc_timer, 0, sizeof(ledc_timer)); ledc_timer.speed_mode = group; ledc_timer.timer_num = LEDC_TIMER_0; ledc_timer.duty_resolution = res; @@ -401,4 +402,4 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); } -#endif \ No newline at end of file +#endif