diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..2c2409bd 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -108,10 +108,11 @@ void loop() { #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" +#include "drivers/StepperDriver8PWM.h" #include "drivers/StepperDriver2PWM.h" -#include "current_sense/InlineCurrentSense.h" -#include "current_sense/LowsideCurrentSense.h" -#include "current_sense/GenericCurrentSense.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" diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8865f57c..b73b39ba 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,5 +1,7 @@ #include "StepperMotor.h" #include "./communication/SimpleFOCDebug.h" +#include "arm_math.h" + // StepperMotor(int pp) @@ -187,19 +189,19 @@ int StepperMotor::alignSensor() { // 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); + _delay(2000); // read the sensor sensor->update(); // get the current zero electric angle zero_electric_angle = 0; zero_electric_angle = electricalAngle(); - _delay(20); + _delay(2000); if(monitor_port){ SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); - _delay(200); + _delay(1000); }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); return exit_flag; } @@ -257,7 +259,7 @@ void StepperMotor::loopFOC() { electrical_angle = electricalAngle(); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage.q, voltage.d, electrical_angle); + setPhaseVoltageCORDIC_LL(voltage.q, voltage.d, electrical_angle); } // Iterative function running outer loop of the FOC algorithm @@ -357,6 +359,8 @@ void StepperMotor::move(float new_target) { void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation +////SerialUSB.println("ange_el -> "); +//SerialUSB.println(angle_el); // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions @@ -367,10 +371,134 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + //set the voltages in hardware + driver->setPwm(Ualpha, Ubeta); +/* + Serial.print("Ualpha:"); + Serial.print(Ualpha, 8); + Serial.print(","); + Serial.print("Ubeta:"); + Serial.print(Ubeta, 8); + Serial.println(); + + */ + +} + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Sine PWM algorithms +// - space vector not implemented yet +// + +int32_t float_to_q31(float input) { + int32_t q31 = (int32_t) (input * 2147483648.0f); + q31 = (q31 > 0) ? (q31 << 1) : (-q31 << 1); + return q31; +} + +#define pi 3.1415926535897932384626433f + +int32_t q31_value; +float value_f32_sine = 0; +float value_f32_cosine = 0; +q31_t cordic_cosine; +q31_t cordic_sine; + +float wrap_to_1(float x) { + while (x > 1.0f) { + x -= 2.0f; + } + while (x < -1.0f) { + x += 2.0f; + } + return x; +} +/// @runger(); +/// @brief +/// @param Uq +/// @param Ud +/// @param angle_el + +#define PI32f 3.141592f + +float cordic_sin_value; +float cordic_cos_value; + +void StepperMotor::setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) { + + + // convert angle flot to CORDICq31 format + uint32_t angle31 = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI)); + + /* Write angle and start CORDIC execution */ + + // WHO U GONNA CALL? CORDIC -> + + CORDIC->WDATA = angle31; + q31_t cosOutput = (int32_t)CORDIC->RDATA; + + // convert q31 result to float + cordic_cos_value = (float)cosOutput / (float)0x80000000; + + /* Read sine */ + q31_t sinOutput = (int32_t)CORDIC->RDATA; + + // convert q31 results to float + cordic_sin_value = (float)sinOutput / (float)0x80000000; + + + +//value_f32_sine = wrap_to_1(value_f32_sine); +//value_f32_cosine = wrap_to_1(value_f32_cosine); + + + // Inverse park transform + //Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq; + //Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq; + + arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, cordic_sin_value, cordic_cos_value); + + + + // set the voltages in hardware + driver->setPwm(Ualpha, Ubeta); + + + +} + + +void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) { + + +uint32_t q1_31_angle = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI)); + + // WHO U GONNA CALL? CORDIC -> +CORDIC->WDATA = q1_31_angle; +cordic_sine = CORDIC->RDATA; +cordic_cosine = CORDIC->RDATA; + +value_f32_sine = (float)cordic_sine/(float)0x80000000; +value_f32_cosine = (float)cordic_cosine/(float)0x80000000; + +value_f32_sine = wrap_to_1(value_f32_sine); +value_f32_cosine = wrap_to_1(value_f32_cosine); + + + // Inverse park transform + //Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq; + //Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq; + + arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, value_f32_sine, value_f32_cosine); + // set the voltages in hardware driver->setPwm(Ualpha, Ubeta); + + + } + // Function (iterative) generating open loop movement for target velocity // - target_velocity - rad/s // it uses voltage_limit variable diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 36f4fe3f..836c0220 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -79,6 +79,8 @@ class StepperMotor: public FOCMotor float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + uint16_t countervalue; //!< counter value for the open loop control TIM3->CNT; + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm @@ -89,10 +91,19 @@ class StepperMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; - private: + // CORDIC + + void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override; + + + void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) override; + + int alignSensor(); + + /** Sensor alignment to electrical 0 angle of the motor */ - int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroSearch(); @@ -113,6 +124,8 @@ class StepperMotor: public FOCMotor float angleOpenloop(float target_angle); // open loop variables long open_loop_timestamp; + + private: }; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c499df4a..3c0d8de7 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -137,6 +137,13 @@ class FOCMotor */ virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0; + + // CORDIC + + virtual void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el)=0; + + virtual void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el)=0; + // State calculation methods /** Shaft angle calculation in radians [rad] */ float shaftAngle(); @@ -157,6 +164,7 @@ class FOCMotor float target; //!< current target value - depends of the controller float shaft_angle;//!< current motor angle float electrical_angle;//!< current electrical angle + float electrical_angle2;//!< current electrical angle float shaft_velocity;//!< current motor velocity float current_sp;//!< target current ( q current ) float shaft_velocity_sp;//!< current target velocity diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 28511762..49df9f2e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -17,14 +17,14 @@ 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; - // TODO handle overflow - we do need to reset vel_angle_prev_ts - 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; - vel_angle_prev = angle_prev; - vel_full_rotations = full_rotations; - vel_angle_prev_ts = angle_prev_ts; + bool isCountingUpward = TIM3->CR1 & 0x0010 ? false : true; + + uint32_t timer2 = TIM2 -> CCR1; + float velocity = (4 * PI / 65536) * 168000000 / ((float)timer2 - 1.0f); + + if (!isCountingUpward) { + velocity = -velocity;} + return velocity; } diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..2a19e238 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -5,28 +5,36 @@ class StepperDriver{ 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 - * - * @param Ua phase A voltage - * @param Ub phase B voltage - */ - virtual void setPwm(float Ua, float Ub) = 0; + /** 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 + +float dc_1a; //!< currently set duty cycle on phaseA +float dc_1b; //!< currently set duty cycle on phaseB +float dc_1c; //!< currently set duty cycle on phaseC +float dc_1d; //!< currently set duty cycle on phaseC + + +/** + * Set phase voltages to the hardware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ +virtual void setPwm(float Ua, float Ub) = 0; + + + }; #endif \ No newline at end of file diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index c38fd897..82546357 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -69,4 +69,4 @@ __attribute__((weak)) float _sqrtApprox(float number) {//low in fat y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; -} +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 40c13e34..7bcd23aa 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -241,7 +241,6 @@ 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); diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp index 376d9d68..7c254ce9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -187,7 +187,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){ #endif #ifdef TIM8 // if defined timer 8 else if(timer->getHandle()->Instance == TIM8) - return ADC_EXTERNALTRIG_T8_TRGO; + return ADC_EXTERNALTRIG_T7_TRGO; #endif #ifdef TIM15 // if defined timer 15 else if(timer->getHandle()->Instance == TIM15) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 4981858f..8a036661 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -55,7 +55,7 @@ int BLDCDriver6PWM::init() { pinMode(pwmB_l, OUTPUT); pinMode(pwmC_l, OUTPUT); if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT); - + Serial.println("Setting Pins to Output!"); // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; @@ -73,6 +73,12 @@ int BLDCDriver6PWM::init() { params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; + + // set phase state to enabled - if driver was successfully initialized + phase_state[0] = PhaseState::PHASE_ON; + phase_state[1] = PhaseState::PHASE_ON; + phase_state[2] = PhaseState::PHASE_ON; + } diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index e8643cc5..e5565322 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -36,6 +36,7 @@ class BLDCDriver6PWM: public BLDCDriver int pwmA_h,pwmA_l; //!< phase A pwm pin number 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; diff --git a/src/drivers/StepperDriver8PWM.cpp b/src/drivers/StepperDriver8PWM.cpp new file mode 100644 index 00000000..6d33e3b9 --- /dev/null +++ b/src/drivers/StepperDriver8PWM.cpp @@ -0,0 +1,141 @@ +#include "StepperDriver8PWM.h" + +StepperDriver8PWM::StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1, int en2, int en3, int en4) { + // Pin initialization + pwm1A = ph1A; + pwm1B = ph1B; + pwm2A = ph2A; + pwm2B = ph2B; + pwm3A = ph3A; + pwm3B = ph3B; + pwm4A = ph4A; + pwm4B = ph4B; + + // Enable pins + enable_pin1 = en1; + enable_pin2 = en2; + enable_pin3 = en3; + enable_pin4 = en4; + + // Default values + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; + + + + pinMode(enable_pin1, OUTPUT); + pinMode(enable_pin2, OUTPUT); + disable(); + + // dead zone initial - 2% + dead_zone = 0.02f; + +} + + +// enable motor driver +void StepperDriver8PWM::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_pin3) ) digitalWrite(enable_pin3, HIGH); + if ( _isset(enable_pin4) ) digitalWrite(enable_pin4, HIGH); + // set zero to PWM + //setPwm(0,0); +} + +// disable motor driver +void StepperDriver8PWM::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_pin3) ) digitalWrite(enable_pin3, LOW); + if ( _isset(enable_pin4) ) digitalWrite(enable_pin4, LOW); + +} + + + + + +// init hardware pins for 8PWM control +int StepperDriver8PWM::init() { + + + + // PWM pins + pinMode(pwm1A, OUTPUT); + pinMode(pwm1B, OUTPUT); + pinMode(pwm2A, OUTPUT); + pinMode(pwm2B, OUTPUT); + pinMode(pwm3A, OUTPUT); + pinMode(pwm3B, OUTPUT); + pinMode(pwm4A, OUTPUT); + pinMode(pwm4B, OUTPUT); + + + + if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT); + if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT); + if( _isset(enable_pin3) ) pinMode(enable_pin3, OUTPUT); + if( _isset(enable_pin4) ) pinMode(enable_pin4, OUTPUT); + + // sanity check for the voltage limit configuration + if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + + phase_state[0] = PhaseState::PHASE_ON; + phase_state[1] = PhaseState::PHASE_ON; + phase_state[2] = PhaseState::PHASE_ON; + phase_state[3] = PhaseState::PHASE_ON; + + // set zero to PWM + dc_1a = dc_1b = dc_1c = dc_1d = 0; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + params = _configure8PWM(pwm_frequency, dead_zone, pwm1A, pwm1B, pwm2A, pwm2B, pwm3A, pwm3B, pwm4A, pwm4B); + + + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; + + + + +} + + + + // Set voltage to the pwm pin for 8PWM control + // Same as 4pwm control, only the phases are setup to be complimentarry. Setting the dutycycle to a complimentarry output, is equal to setting PWM to PWM 1-line (PWM) FET driver + void StepperDriver8PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); + + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + + + // hardware specific writing + if( Ualpha > 0 ) + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + else + duty_cycle2B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + + if( Ubeta > 0 ) + duty_cycle1B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + else + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + + + // write to hardware + _writeDutyCycle8PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, phase_state, params); + } + + //Yes, with a duty cycle of 30%, and reversed polarity, the high side will be on for + //30% and off for 70%, and the low side will be off for 30% and on for 70%. \ No newline at end of file diff --git a/src/drivers/StepperDriver8PWM.h b/src/drivers/StepperDriver8PWM.h new file mode 100644 index 00000000..fc70f590 --- /dev/null +++ b/src/drivers/StepperDriver8PWM.h @@ -0,0 +1,68 @@ +#ifndef STEPPER_DRIVER_8PWM_h +#define STEPPER_DRIVER_8PWM_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 8 pwm stepper driver class +*/ +class StepperDriver8PWM : public StepperDriver { + public: + /** + StepperMotor class constructor + @param ph1A 1A phase pwm pin + @param ph1B 1B phase pwm pin + @param ph2A 2A phase pwm pin + @param ph2B 2B phase pwm pin + @param ph3A 3A phase pwm pin + @param ph3B 3B phase pwm pin + @param ph4A 4A phase pwm pin + @param ph4B 4B phase pwm pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + @param en3 enable pin phase 3 (optional input) + @param en4 enable pin phase 4 (optional input) + */ + StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int en4 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] + + PhaseState phase_state[4]; //!< phase state (active / disabled) + + // hardware variables + int pwm1A, pwm1B; //!< phase 1A pwm pin number + int pwm2A, pwm2B; + int pwm3A, pwm3B; + int pwm4A, pwm4B; + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + int enable_pin3; //!< enable pin number phase 3 + int enable_pin4; //!< enable pin number phase 4 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + + private: + +}; + + + +#endif diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..54f65acf 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -18,11 +18,11 @@ #endif // used for 6-PWM mode, high-side #ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH -#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true +#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH false #endif // used for 6-PWM mode, low-side #ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH -#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true +#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH false #endif @@ -36,7 +36,7 @@ // will be returned as a void pointer from the _configurexPWM functions // will be provided to the _writeDutyCyclexPWM() as a void pointer typedef struct GenericDriverParams { - int pins[6]; + int pins[8]; long pwm_frequency; float dead_zone; } GenericDriverParams; @@ -114,6 +114,26 @@ 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); +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 8PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + * @param pin3A pin3A stepper driver + * @param pin3B pin3B stepper driver + * @param pin4A pin4A stepper driver + * @param pin4B pin4B stepper driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful + */ +void* _configure8PWM(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, const int pinD_h, const int pinD_l); + + /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) * - Stepper driver - 2PWM setting @@ -177,4 +197,23 @@ 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 setting the duty cycle to the pwm pin (ex. analogWrite()) +* - Stepper driver - 8PWM setting +* - hardware specific +*@param dc_1a duty cycle phase 1A [0, 1] +*@param dc_1b duty cycle phase 1B [0, 1] +*@param dc_1c duty cycle phase 1C [0, 1] +*@param dc_1d duty cycle phase 1D [0, 1] +*@param dc_2a duty cycle phase 2A [0, 1] +*@param dc_2b duty cycle phase 2B [0, 1] +*@param dc_2c duty cycle phase 2C [0, 1] +*@param dc_2d duty cycle phase 2D [0, 1] +*@param params the driver parameters +*/ + +void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_1c, float dc_1d, PhaseState *phase_state, void* params); + + #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 d16808f6..4aa8ce98 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -185,6 +185,8 @@ void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, } + + // align the timers to end the init void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) { @@ -251,9 +253,10 @@ void _alignTimersNew() { - // configure hardware 6pwm for a complementary pair of channels STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { + + // sanity check if (pinH==NP || pinL==NP) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; @@ -264,39 +267,48 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* uint32_t channel1 = STM_PIN_CHANNEL(pinH->function); uint32_t channel2 = STM_PIN_CHANNEL(pinL->function); - + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0}; // more sanity check if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral); - + uint32_t OCState = get_timer_index((TIM_TypeDef*)pinH->peripheral); if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - // HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + //HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + LL_TIM_EnableCounter(HardwareTimer_Handle[index]->handle.Instance); + + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT); + + } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin); HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin); + //TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_ENABLE; + //TIM_OC_InitStruct.OCNPolarity = TIM_OCNPOLARITY_LOW; + // 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); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! - #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false + #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==true LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); #endif - #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false + #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==true LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinL), LL_TIM_OCPOLARITY_LOW); #endif LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); HT->pause(); - + // make sure timer output goes to LOW when timer channels are temporarily disabled LL_TIM_SetOffStates(HT->getHandle()->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); @@ -310,6 +322,9 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* + + + STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { STM32DriverParams* params = new STM32DriverParams { .timers = { NP, NP, NP, NP, NP, NP }, @@ -330,6 +345,30 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi } +STM32DriverParams* _initHardware8PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l, PinMap* pinD_h, PinMap* pinD_l) { + STM32DriverParams* params = new STM32DriverParams { + .timers = { NP, NP, NP, NP, NP, NP, NP, NP }, + .channels = { 0, 0, 0, 0, 0, 0, 0, 0 }, + .pwm_frequency = PWM_freq, + .dead_zone = dead_zone, + .interface_type = _HARDWARE_8PWM + }; + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinD_h, pinD_l, params, 6) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + + return params; +} @@ -378,7 +417,7 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) { } if (!found) score++; } - if (numPins==6) { + if (numPins==8) { // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels // >1 timer, 3 channels, 3 matching inverted channels // 1 timer, 6 channels (no inverted channels) @@ -392,7 +431,7 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) { && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) - && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { + && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function) && STM_PIN_INVERTED(pinTimers[7]->function)) { // hardware 6pwm, score <10 // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 @@ -478,6 +517,7 @@ int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTime score = scoreCombination(numPins, searchArray); #ifdef SIMPLEFOC_STM32_DEBUG printTimerCombination(numPins, searchArray, score); + delay(50); #endif } if (score==-1) @@ -507,6 +547,7 @@ int findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]) { #ifdef SIMPLEFOC_STM32_DEBUG SimpleFOCDebug::print("STM32-DRV: best: "); printTimerCombination(numPins, pinTimers, bestScore); + delay(50); #endif } return bestScore; @@ -621,6 +662,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in .channels = { channel1, channel2, channel3 }, .pwm_frequency = pwm_frequency }; + + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; @@ -632,47 +675,23 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in -// 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) { - if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); - return (STM32DriverParams*)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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; - int pins[4] = { pinA, pinB, pinC, pinD }; - PinMap* pinTimers[4] = { NP, NP, NP, NP }; - if (findBestTimerCombination(4, pins, pinTimers)<0) - return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); - // allign the timers - _alignPWMTimers(HT1, HT2, HT3, HT4); - 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); - uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); - STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2, HT3, HT4 }, - .channels = { channel1, channel2, channel3, channel4 }, - .pwm_frequency = pwm_frequency - }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[3]; - return params; -} +/* + +Yes, it makes sense to have these two parts next to each other. The first part of the code initializes and starts the PWM outputs of the timers (TIM1 and TIM8), + while the second part of the code configures the timers for PWM output and sets their various parameters, such as duty cycle, frequency, and so on. + +By having these two parts of the code next to each other, the programmer can ensure that the timers are properly configured and started before any other code is executed. +This can be important in systems where the timing of signals is critical, as it can prevent race conditions and other timing issues that might arise if the timers were not properly configured +and started before other code began executing. + +Furthermore, the second part of the code relies on the first part to have started the timers before it can properly configure them. Therefore, it is logical to have these two parts of +the code next to each other to ensure that the timers are properly initialized and configured for PWM output. + +*/ @@ -720,6 +739,48 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo + void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) { + _UNUSED(channel2); + switch (state) { + case PhaseState::PHASE_OFF: + // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE). + // To actually make the phase floating, we must also set pwm to 0. + HT->pauseChannel(channel1); + break; + default: + HT->resumeChannel(channel1); + break; + } +} + + + + + + + +/* + +Yes, you can call the "__HAL_TIM_SET_COMPARE" function without calling "HAL_TIM_PWM_Start" +and "HAL_TIMEx_PWMN_Start" functions every time, as long as the timer +and channel configuration have been properly set up beforehand. + +The "__HAL_TIM_SET_COMPARE" function is used to set the compare value for a specific +timer channel. This function does not start the PWM output on the channel, it simply +sets the compare value. Once the compare value is set, the timer will automatically +generate PWM output on the corresponding channel when the timer counter reaches the +compare value. + +Therefore, if you have already set up the timer and channel configuration and called +the "HAL_TIM_PWM_Start" and "HAL_TIMEx_PWMN_Start" functions once, you do not need to call +them again every time you update the compare value using "__HAL_TIM_SET_COMPARE". You can +simply call "__HAL_TIM_SET_COMPARE" to update the compare value and the timer will generate the + PWM output automatically on the corresponding channel. +*/ + + + + // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific @@ -729,7 +790,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons return (STM32DriverParams*)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 |%0kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; @@ -765,37 +826,76 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons .interface_type = _SOFTWARE_6PWM }; } + +/* if (score>=0) { for (int i=0; i<6; i++) timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; _alignTimersNew(); } + + */ return params; // success } +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 8PWM setting +// - hardware specific +void* _configure8PWM(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, const int pinD_h, const int pinD_l ){ + if (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)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 |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + // find configuration + int pins[8] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l, pinD_h, pinD_l}; + + + for(int i = 0; i < 8; i++) +{ + Serial.println(pins[i]); +} + PinMap* pinTimers[8] = { NP, NP, NP, NP, NP, NP, NP, NP}; + + int score = findBestTimerCombination(8, pins, pinTimers); + + -void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) { - _UNUSED(channel2); - switch (state) { - case PhaseState::PHASE_OFF: - // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE). - // To actually make the phase floating, we must also set pwm to 0. - HT->pauseChannel(channel1); - break; - default: - HT->resumeChannel(channel1); - break; - } + STM32DriverParams* params; + // configure accordingly + if (score<0) + params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + else if (score<10) // hardware pwm + params = _initHardware8PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5], pinTimers[6], pinTimers[7]); + +/* + if (score>=0) { + for (int i=0; i<8; i++) + timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; + _alignTimersNew(); + } */ + + return params; // success } + + + + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState* phase_state, void* params){ switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: + + // phase a _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]); if(phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f; @@ -843,6 +943,33 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState* phase_s } +void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_1c, float dc_1d, PhaseState* phase_state, void* params) { + + // Scale duty cycles to the PWM + + + _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]); + if(phase_state[0] == PhaseState::PHASE_OFF) dc_1a = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + // phase b + + _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], ((STM32DriverParams*)params)->channels[3]); + if(phase_state[1] == PhaseState::PHASE_OFF) dc_1b = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + + // phase c + _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], ((STM32DriverParams*)params)->channels[5]); + if(phase_state[2] == PhaseState::PHASE_OFF) dc_1c = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_1c, _PWM_RESOLUTION); + + _setSinglePhaseState(phase_state[3], ((STM32DriverParams*)params)->timers[6], ((STM32DriverParams*)params)->channels[6], ((STM32DriverParams*)params)->channels[7]); + if(phase_state[3] == PhaseState::PHASE_OFF) dc_1d = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[6], ((STM32DriverParams*)params)->channels[6], _PWM_RANGE*dc_1d, _PWM_RESOLUTION); + + } + + + #ifdef SIMPLEFOC_STM32_DEBUG @@ -932,6 +1059,7 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { SimpleFOCDebug::print(" "); } SimpleFOCDebug::println("score: ", score); + delay(100); } #endif diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..0db80cb5 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -15,10 +15,17 @@ #define _SOFTWARE_6PWM 0 #define _ERROR_6PWM -1 +// 8pwm parameters +#define _HAS_CORDIC 0 +#define _USE_CORDIC 0 +#define _HARDWARE_8PWM 1 +#define _SOFTWARE_8PWM 0 +#define _ERROR_8PWM -1 + typedef struct STM32DriverParams { - HardwareTimer* timers[6] = {NULL}; - uint32_t channels[6]; + HardwareTimer* timers[8] = {NULL}; + uint32_t channels[8]; long pwm_frequency; float dead_zone; uint8_t interface_type;