From e7e92e7603e7b884c8e68ae282c07ad7a7936112 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 19:04:40 +0100 Subject: [PATCH 01/29] =?UTF-8?q?dev=C2=B4s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/StepperDriver8PWM.cpp | 102 ++++++++++++++++++++++++++++++ src/drivers/StepperDriver8PWM.h | 68 ++++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 src/drivers/StepperDriver8PWM.cpp create mode 100644 src/drivers/StepperDriver8PWM.h diff --git a/src/drivers/StepperDriver8PWM.cpp b/src/drivers/StepperDriver8PWM.cpp new file mode 100644 index 00000000..1ec2b751 --- /dev/null +++ b/src/drivers/StepperDriver8PWM.cpp @@ -0,0 +1,102 @@ +#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; + +} + + + + + + + + +// 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); + + // sanity check for the voltage limit configuration + if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // 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 +void StepperDriver8PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A_h1(0.0f),duty_cycle1A_h2(0.0f),duty_cycle1B_h1(0.0f),duty_cycle1B_h2(0.0f); + float duty_cycle2A_h1(0.0f),duty_cycle2A_h2(0.0f),duty_cycle2B_h1(0.0f),duty_cycle2B_h2(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_cycle1B_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + duty_cycle1B_h2 = 0.0f; // set second half-bridge duty cycle to 0 + } + else { + duty_cycle1A_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + duty_cycle1A_h2 = 0.0f; // set second half-bridge duty cycle to 0 + } + + if( Ubeta > 0 ) { + duty_cycle2B_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + duty_cycle2B_h2 = 0.0f; // set second half-bridge duty cycle to 0 + } + else { + duty_cycle2A_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + duty_cycle2A_h2 = 0.0f; // set second half-bridge duty cycle to 0 + } + + // write to hardware + _writeDutyCycle8PWM(duty_cycle1A_h1, duty_cycle1A_h2, duty_cycle1B_h1, duty_cycle1B_h2, + duty_cycle2A_h1, duty_cycle2A_h2, duty_cycle2B_h1, duty_cycle2B_h2, params); +} diff --git a/src/drivers/StepperDriver8PWM.h b/src/drivers/StepperDriver8PWM.h new file mode 100644 index 00000000..d4056bfa --- /dev/null +++ b/src/drivers/StepperDriver8PWM.h @@ -0,0 +1,68 @@ +#ifndef STEPPER_DRIVER_4PWM_h +#define STEPPER_DRIVER_4PWM_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] + + // hardware variables + int pwm1A; //!< phase 1A pwm pin number + int pwm1B; //!< phase 1B pwm pin number + int pwm2A; //!< phase 2A pwm pin number + int pwm2B; //!< phase 2B pwm pin number + int pwm3A; //!< phase 3A pwm pin number + int pwm3B; //!< phase 3B pwm pin number + int pwm4A; //!< phase 4A pwm pin number + int pwm4B; //!< phase 4B pwm pin number + 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 From a520bc1d0b031fb8477b35ff7014bff9fffa4e60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 19:12:09 +0100 Subject: [PATCH 02/29] Initial 8PWM --- src/StepperMotor.cpp | 64 ++++++++ src/StepperMotor.h | 4 + src/common/base_classes/StepperDriver.h | 46 +++--- src/drivers/hardware_api.h | 42 ++++- .../hardware_specific/stm32/stm32_mcu.cpp | 153 ++++++++++++++---- .../hardware_specific/stm32/stm32_mcu.h | 9 +- 6 files changed, 260 insertions(+), 58 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8865f57c..0e7bbaa6 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -371,6 +371,70 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { driver->setPwm(Ualpha, Ubeta); } +// 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; +float cordic_cosine = 0.0f; +float cordic_sine = 0.0f; + +float wrap_to_1(float x) { + while (x > 1.0f) { + x -= 2.0f; + } + while (x < -1.0f) { + x += 2.0f; + } + return x; +} + +void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) { + // Sinusoidal PWM modulation + // Inverse Park transformation + + // WHO U GONNA CALL? CORDIC -> + +q31_value = float_to_q31(angle_el / (2.0f * pi)); + +CORDIC->WDATA = q31_value; +cordic_sine = CORDIC->RDATA; +cordic_cosine = CORDIC->RDATA; + +value_f32_sine = (float)cordic_sine/(float)0x80000000; +value_f32_cosine = (float)cordic_cosine/(float)0x80000000; + +if (angle < 0){ +value_f32_sine = wrap_to_1(value_f32_sine); +value_f32_sine = value_f32_sine * -1; +} + +if (angle > 0){ +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 - cordic_sine * Uq; // -sin(angle) * Uq; + Ubeta = cordic_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq; + + // 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..94e002a2 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -89,6 +89,10 @@ class StepperMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + // CORDIC + + void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override; + private: /** Sensor alignment to electrical 0 angle of the motor */ diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..4dd7203c 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -5,28 +5,30 @@ 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 + +/** + * 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/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..b9065293 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -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,27 @@ 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 pin1A, const int pin1B, const int pin2A, const int pin2B, + const int pin3A, const int pin3B, const int pin4A, const int pin4B); + + /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) * - Stepper driver - 2PWM setting @@ -177,4 +198,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, float dc_2a, float dc_2b, float dc_2c, float dc_2d, 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..dc39f3cf 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -184,6 +184,36 @@ void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HT4->resume(); } +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4, HardwareTimer *HT5, HardwareTimer *HT6, HardwareTimer *HT7, HardwareTimer *HT8) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT5->pause(); + HT5->refresh(); + HT6->pause(); + HT6->refresh(); + HT7->pause(); + HT7->refresh(); + HT8->pause(); + HT8->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); + HT5->resume(); + HT6->resume(); + HT7->resume(); + HT8->resume(); +} + + // align the timers to end the init void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) @@ -251,7 +281,6 @@ 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 @@ -310,6 +339,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 +362,26 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi } +STM32DriverParams* _initHardware8PWMInterface(long PWM_freq, float dead_zone, PinMap* pin1A, PinMap* pin1B, PinMap* pin2A, PinMap* pin2B, PinMap* pin3A, PinMap* pin3B, PinMap* pin4A, PinMap* pin4B) { + 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, pin1A, pin1B, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if (_initHardware6PWMPair(PWM_freq, dead_zone, pin2A, pin2B, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if (_initHardware6PWMPair(PWM_freq, dead_zone, pin3A, pin3B, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if (_initHardware6PWMPair(PWM_freq, dead_zone, pin4A, pin4B, params, 6) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + return params; +} @@ -621,6 +673,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,46 +686,66 @@ 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) { +// - Stepper motor - 8PWM setting +// - hardware specific +void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B, + const int pin3A, const int pin3B, const int pin4A, const int pin4B) +{ + if (numTimerPinsUsed+8 > 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 + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz 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); + // find configuration + int pins[8] = { pin1A, pin1B, pin2A, pin2B, pin3A, pin3B, pin4A, pin4B }; + PinMap* pinTimers[8] = { NP, NP, NP, NP, NP, NP, NP, NP }; + int score = findBestTimerCombination(8, pins, pinTimers); - 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; + 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]); + else { // software pwm + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); + HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); + HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); + HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + HardwareTimer* HT7 = _initPinPWMHigh(pwm_frequency, pinTimers[6]); + HardwareTimer* HT8 = _initPinPWMLow(pwm_frequency, pinTimers[7]); + 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); + uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); + uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); + uint32_t channel7 = STM_PIN_CHANNEL(pinTimers[6]->function); + uint32_t channel8 = STM_PIN_CHANNEL(pinTimers[7]->function); + params = new STM32DriverParams { + + .timers = { HT1, HT2, HT3, HT4, HT5, HT6, HT7, HT8}, + .channels = { channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone, + .interface_type = _SOFTWARE_8PWM + }; + } + if (score>=0) { + for (int i=0; i<8; i++) + timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; + _alignTimersNew(); + } + return params; // success } @@ -717,7 +791,20 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); } - +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 8PWM setting +// - hardware specific +void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, float dc_3a, float dc_3b, float dc_4a, float dc_4b, void* params) { + // transform duty cycle from [0,1] to [0,4095] + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE * dc_1a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE * dc_1b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE * dc_2a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE * dc_2b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE * dc_3a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _PWM_RANGE * dc_3b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[6], ((STM32DriverParams*)params)->channels[6], _PWM_RANGE * dc_4a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[7], ((STM32DriverParams*)params)->channels[7], _PWM_RANGE * dc_4b, _PWM_RESOLUTION); +} // Configuring PWM frequency, resolution and alignment diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..d3d9e1ff 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -15,10 +15,15 @@ #define _SOFTWARE_6PWM 0 #define _ERROR_6PWM -1 +// 8pwm parameters +#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; From 44f2ecc6548940f480d8870d6c7399e97b2c34b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 21:42:58 +0100 Subject: [PATCH 03/29] Update FOCMotor.h --- src/common/base_classes/FOCMotor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c499df4a..d8aaa45f 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -137,6 +137,11 @@ 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; + // State calculation methods /** Shaft angle calculation in radians [rad] */ float shaftAngle(); From ef09b21ae8f4ce6925628d3c653eeeb33bf1e2e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 22:05:39 +0100 Subject: [PATCH 04/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 32 +------------------ 1 file changed, 1 insertion(+), 31 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index dc39f3cf..5c5ecb59 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -184,34 +184,6 @@ void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HT4->resume(); } -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4, HardwareTimer *HT5, HardwareTimer *HT6, HardwareTimer *HT7, HardwareTimer *HT8) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT4->pause(); - HT4->refresh(); - HT5->pause(); - HT5->refresh(); - HT6->pause(); - HT6->refresh(); - HT7->pause(); - HT7->refresh(); - HT8->pause(); - HT8->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); - HT4->resume(); - HT5->resume(); - HT6->resume(); - HT7->resume(); - HT8->resume(); -} @@ -686,8 +658,6 @@ 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 - 8PWM setting // - hardware specific @@ -699,7 +669,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const 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; From 160f199f3efce4ece145f1d838f30ee24d3283f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 22:12:32 +0100 Subject: [PATCH 05/29] Update StepperDriver8PWM.h --- src/drivers/StepperDriver8PWM.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/StepperDriver8PWM.h b/src/drivers/StepperDriver8PWM.h index d4056bfa..34833f5d 100644 --- a/src/drivers/StepperDriver8PWM.h +++ b/src/drivers/StepperDriver8PWM.h @@ -1,5 +1,5 @@ -#ifndef STEPPER_DRIVER_4PWM_h -#define STEPPER_DRIVER_4PWM_h +#ifndef STEPPER_DRIVER_8PWM_h +#define STEPPER_DRIVER_8PWM_h #include "../common/base_classes/StepperDriver.h" #include "../common/foc_utils.h" From ced13e34e028a4c13e9ce5c5d91c6e6c837760e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Tue, 21 Mar 2023 22:32:39 +0100 Subject: [PATCH 06/29] Update foc_utils.cpp --- src/common/foc_utils.cpp | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 11a1810f..82546357 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){ +__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 @@ -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){ +__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); @@ -44,7 +45,7 @@ float _cos(float a){ // normalizing radian angle to [0,2PI] -float _normalizeAngle(float angle){ +__attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } @@ -57,17 +58,15 @@ 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; +__attribute__((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 return number * y; -} +} \ No newline at end of file From 271de7d53a4426c93e8ef193512ce003beb4c9c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 01:39:53 +0100 Subject: [PATCH 07/29] Update StepperMotor.cpp --- src/StepperMotor.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 0e7bbaa6..33148e7b 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -399,7 +399,11 @@ float wrap_to_1(float x) { } return x; } - +/// @runger(); +/// @brief +/// @param Uq +/// @param Ud +/// @param angle_el void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation @@ -415,20 +419,20 @@ cordic_cosine = CORDIC->RDATA; value_f32_sine = (float)cordic_sine/(float)0x80000000; value_f32_cosine = (float)cordic_cosine/(float)0x80000000; -if (angle < 0){ +if (angle_el < 0){ value_f32_sine = wrap_to_1(value_f32_sine); value_f32_sine = value_f32_sine * -1; } -if (angle > 0){ +if (angle_el > 0){ 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 - cordic_sine * Uq; // -sin(angle) * Uq; - Ubeta = cordic_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq; + Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq; + Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq; // set the voltages in hardware driver->setPwm(Ualpha, Ubeta); From d8d615872febb07c7d2d6362f33e1ad6c4cd9a41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 13:02:07 +0100 Subject: [PATCH 08/29] Update stm32_mcu.h The test breaks becouse most MCUs does not have the CORDIC. It needs to be contained within these defines. --- src/drivers/hardware_specific/stm32/stm32_mcu.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index d3d9e1ff..0db80cb5 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -16,6 +16,8 @@ #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 From 5dbf3d0aa10cc7ad6c012efaaaf403a8b0f1ba46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 18:07:50 +0100 Subject: [PATCH 09/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 166 ++++++++++++------ 1 file changed, 109 insertions(+), 57 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 5c5ecb59..bc73f674 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -658,65 +658,117 @@ 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 - 8PWM setting -// - hardware specific -void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B, - const int pin3A, const int pin3B, const int pin4A, const int pin4B) -{ - if (numTimerPinsUsed+8 > 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; - // find configuration - int pins[8] = { pin1A, pin1B, pin2A, pin2B, pin3A, pin3B, pin4A, pin4B }; - PinMap* pinTimers[8] = { NP, NP, NP, NP, NP, NP, NP, NP }; - int score = findBestTimerCombination(8, pins, pinTimers); +#include "stm32g4xx_hal.h" +#include "stm32g4xx_hal_gpio.h" +#include "stm32g4xx_hal_tim.h" +#include "stm32g4xx_hal_tim_ex.h" - 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]); - else { // software pwm - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); - HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); - HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); - HardwareTimer* HT7 = _initPinPWMHigh(pwm_frequency, pinTimers[6]); - HardwareTimer* HT8 = _initPinPWMLow(pwm_frequency, pinTimers[7]); - 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); - uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); - uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); - uint32_t channel7 = STM_PIN_CHANNEL(pinTimers[6]->function); - uint32_t channel8 = STM_PIN_CHANNEL(pinTimers[7]->function); - params = new STM32DriverParams { - - .timers = { HT1, HT2, HT3, HT4, HT5, HT6, HT7, HT8}, - .channels = { channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 }, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone, - .interface_type = _SOFTWARE_8PWM - }; - } - if (score>=0) { - for (int i=0; i<8; i++) - timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; - _alignTimersNew(); - } - return params; // success -} +TIM_HandleTypeDef htim1; +TIM_HandleTypeDef htim8; + +void configure8PWM(void) +{ + + HAL_Init(); + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + // Enable clock for TIM1 and TIM8 + __HAL_RCC_TIM1_CLK_ENABLE(); + __HAL_RCC_TIM8_CLK_ENABLE(); + + // Configure TIM1 pins + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_7 | GPIO_PIN_9 | GPIO_PIN_6 | GPIO_PIN_0 | GPIO_PIN_1; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + // Configure TIM8 pins + GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + // Set TIM1 and TIM8 dead time values to 50 ns + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + htim1.Instance = TIM1; + htim1.Init.Prescaler = 0; + htim1.Init.CounterMode = TIM_COUNTERMODE_UP; + htim1.Init.Period = 100; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter= 0; + HAL_TIM_PWM_Init(&htim1); + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 50; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); + HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + + // Continue with configuring TIM8 + htim8.Instance = TIM8; + htim8.Init.Prescaler = 0; + htim8.Init.CounterMode = TIM_COUNTERMODE_UP; + htim8.Init.Period = 100; + htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim8.Init.RepetitionCounter= 0; + HAL_TIM_PWM_Init(&htim8); + HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); + HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig); + + // Configure TIM1 and TIM8 PWM channels + TIM_OC_InitTypeDef sConfigOC = {0}; + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + + // TIM1 Channel 1 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); + + // TIM1 Channel 2 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); + + // TIM1 Channel 3 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); + + // TIM1 Channel 4 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); + + // TIM1 Channel 5 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); + + // TIM1 Channel 6 + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); + + // TIM8 Channel 1 + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); + + // TIM8 Channel 2 + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2); + } From 417507e59cada36a0d96d562b0185557987b667b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 18:56:31 +0100 Subject: [PATCH 10/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 155 +++++++++--------- 1 file changed, 76 insertions(+), 79 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index bc73f674..929f6b3e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -669,106 +669,103 @@ TIM_HandleTypeDef htim8; void configure8PWM(void) { - HAL_Init(); GPIO_InitTypeDef GPIO_InitStruct = {0}; - + // Enable clock for TIM1 and TIM8 __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - + // Configure TIM1 pins - GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_7 | GPIO_PIN_9 | GPIO_PIN_6 | GPIO_PIN_0 | GPIO_PIN_1; + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - + GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; + HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); + // Configure TIM8 pins - GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; + GPIO_InitStruct.Pin = GPIO_PIN_0; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_14; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + // Set TIM1 and TIM8 dead time values to 50 ns TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; htim1.Instance = TIM1; htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 100; + htim1.Init.Period = 255; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter= 0; + htim1.Init.RepetitionCounter = 0; HAL_TIM_PWM_Init(&htim1); - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 50; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); - HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); - - // Continue with configuring TIM8 - htim8.Instance = TIM8; - htim8.Init.Prescaler = 0; - htim8.Init.CounterMode = TIM_COUNTERMODE_UP; - htim8.Init.Period = 100; - htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim8.Init.RepetitionCounter= 0; - HAL_TIM_PWM_Init(&htim8); - HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); - HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig); - - // Configure TIM1 and TIM8 PWM channels - TIM_OC_InitTypeDef sConfigOC = {0}; - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; - - // TIM1 Channel 1 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); - - // TIM1 Channel 2 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); - - // TIM1 Channel 3 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); - - // TIM1 Channel 4 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); - - // TIM1 Channel 5 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); - - // TIM1 Channel 6 - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); - - // TIM8 Channel 1 - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); - - // TIM8 Channel 2 - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2); - } + + +htim8.Instance = TIM8; +htim8.Init.Prescaler = 0; +htim8.Init.CounterMode = TIM_COUNTERMODE_UP; +htim8.Init.Period = 255; +htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; +htim8.Init.RepetitionCounter = 0; +HAL_TIM_PWM_Init(&htim8); + +sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; +sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; +HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + +sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; +sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; +sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; +sBreakDeadTimeConfig.DeadTime = 50; +sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; +sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; +sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; +HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); +HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); + +// Configure PWM mode for TIM1 channels 1-6 +TIM_OC_InitTypeDef sConfigOC = {0}; +sConfigOC.OCMode = TIM_OCMODE_PWM1; +sConfigOC.Pulse = 0; +sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; +sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); +HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); + +// Configure PWM mode for TIM8 channel 1 +sConfigOC.OCMode = TIM_OCMODE_PWM1; +sConfigOC.Pulse = 0; +sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; +sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; +HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); + +// Start PWM output +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); +HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); +HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); + + +} + + + + + + + From cf4bfecc0acacac86047d6ab1eb163a39c2a10dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 19:13:19 +0100 Subject: [PATCH 11/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 105 +++++++++--------- 1 file changed, 54 insertions(+), 51 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 929f6b3e..1ce62993 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -705,57 +705,60 @@ void configure8PWM(void) htim1.Init.RepetitionCounter = 0; HAL_TIM_PWM_Init(&htim1); - -htim8.Instance = TIM8; -htim8.Init.Prescaler = 0; -htim8.Init.CounterMode = TIM_COUNTERMODE_UP; -htim8.Init.Period = 255; -htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; -htim8.Init.RepetitionCounter = 0; -HAL_TIM_PWM_Init(&htim8); - -sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; -sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; -HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); - -sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; -sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; -sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; -sBreakDeadTimeConfig.DeadTime = 50; -sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; -sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; -sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; -HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); -HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); - -// Configure PWM mode for TIM1 channels 1-6 -TIM_OC_InitTypeDef sConfigOC = {0}; -sConfigOC.OCMode = TIM_OCMODE_PWM1; -sConfigOC.Pulse = 0; -sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; -sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); -HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); - -// Configure PWM mode for TIM8 channel 1 -sConfigOC.OCMode = TIM_OCMODE_PWM1; -sConfigOC.Pulse = 0; -sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; -sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; -HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); - -// Start PWM output -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); -HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); -HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); + htim8.Instance = TIM8; + htim8.Init.Prescaler = 0; + htim8.Init.CounterMode = TIM_COUNTERMODE_UP; + htim8.Init.Period = 255; + htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim8.Init.RepetitionCounter = 0; + HAL_TIM_PWM_Init(&htim8); + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 50; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); + HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); + + // Configure PWM mode for TIM1 channels 1-6 + TIM_OC_InitTypeDef sConfigOC = {0}; + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); + + // Configure PWM mode for TIM8 channel 1 + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); + + // Start TIM1 and TIM8 PWM signals + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); + HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); } From 014ab05878ebea0df11a18560cb89780ddd79a8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 19:49:51 +0100 Subject: [PATCH 12/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 62 +++++++++---------- 1 file changed, 30 insertions(+), 32 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 1ce62993..c3273ed0 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -664,33 +664,38 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in #include "stm32g4xx_hal_tim.h" #include "stm32g4xx_hal_tim_ex.h" +// Declare timer handles for TIM1 and TIM8 TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim8; +// Function to configure PWM output on TIM1 channels 1-6 and TIM8 channel 1 void configure8PWM(void) { + // Initialize HAL library HAL_Init(); + // GPIO pin initialization struct GPIO_InitTypeDef GPIO_InitStruct = {0}; // Enable clock for TIM1 and TIM8 __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - // Configure TIM1 pins + // Configure TIM1 pins for alternate function mode with push-pull output GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - // Configure TIM8 pins + // Configure TIM8 pins for alternate function mode with push-pull output GPIO_InitStruct.Pin = GPIO_PIN_0; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + // Configure additional TIM8 pin for PWM output GPIO_InitStruct.Pin = GPIO_PIN_14; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -725,42 +730,35 @@ void configure8PWM(void) sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); - HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig); + - // Configure PWM mode for TIM1 channels 1-6 - TIM_OC_InitTypeDef sConfigOC = {0}; - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_5); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_6); - // Configure PWM mode for TIM8 channel 1 + // Configure TIM1 channels 1-6 for PWM output + TIM_OC_InitTypeDef sConfigOC = {0}; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1); - - // Start TIM1 and TIM8 PWM signals - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_5); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_6); - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); - - + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH1); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH1N); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH3); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH3N); + + // Configure PWM output on TIM8 channel 1 and additional channel + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2); + + // Start TIM1 and TIM8 PWM outputs + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1N); + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3); + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3N); + HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); } From d4da3f51ca48ddecf1b4ed140323b4dd552fbd4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 20:30:41 +0100 Subject: [PATCH 13/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index c3273ed0..5fe6860e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -681,23 +681,25 @@ void configure8PWM(void) __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - // Configure TIM1 pins for alternate function mode with push-pull output - GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; - HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); + // Configure GPIO pins for STSPIN32G4 internal FET driver + + __HAL_RCC_GPIOE_CLK_ENABLE(); // enable GPIO clock + GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_13 | GPIO_PIN_12; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // alternate function mode with push-pull output + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; + HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); // Configure TIM8 pins for alternate function mode with push-pull output - GPIO_InitStruct.Pin = GPIO_PIN_0; + GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_0; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - // Configure additional TIM8 pin for PWM output - GPIO_InitStruct.Pin = GPIO_PIN_14; + GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + // Set TIM1 and TIM8 dead time values to 50 ns TIM_MasterConfigTypeDef sMasterConfig = {0}; @@ -747,8 +749,8 @@ void configure8PWM(void) HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH3N); // Configure PWM output on TIM8 channel 1 and additional channel - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); // Start TIM1 and TIM8 PWM outputs HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); @@ -757,8 +759,8 @@ void configure8PWM(void) HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2N); HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3); HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3N); - HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); } From 9da76da15a2493c4542a59e3880c0c704419476c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Wed, 22 Mar 2023 20:40:19 +0100 Subject: [PATCH 14/29] Update stm32_mcu.cpp --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 5fe6860e..958c7688 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -733,6 +733,16 @@ void configure8PWM(void) sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); + // Set TIM8 dead time values to 50 ns + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfigTIM8 = {0}; + sBreakDeadTimeConfigTIM8.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfigTIM8.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfigTIM8.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfigTIM8.DeadTime = 50; + sBreakDeadTimeConfigTIM8.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfigTIM8.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfigTIM8.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfigTIM8); // Configure TIM1 channels 1-6 for PWM output From cb92a7488f9fc80fe010df78f4931f08bfd21cc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 01:42:45 +0100 Subject: [PATCH 15/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 127 +++++++++++++++--- 1 file changed, 107 insertions(+), 20 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 958c7688..0779f37d 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -657,22 +657,22 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in } - - #include "stm32g4xx_hal.h" #include "stm32g4xx_hal_gpio.h" #include "stm32g4xx_hal_tim.h" #include "stm32g4xx_hal_tim_ex.h" + + + // Declare timer handles for TIM1 and TIM8 TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim8; // Function to configure PWM output on TIM1 channels 1-6 and TIM8 channel 1 -void configure8PWM(void) -{ - // Initialize HAL library - HAL_Init(); +void* _configure8PWM(long pwm_frequency, float dead_zone) +{ + // GPIO pin initialization struct GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -771,6 +771,34 @@ void configure8PWM(void) HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3N); HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); + + + +TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable +TIM1->CR1 &= ~TIM_CR1_DIR; // Up counting mode +TIM1->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 +TIM1->CCER |= TIM_CCER_CC1E; // Enable output on OC1 +TIM1->PSC = 0; // Set prescaler to 0 +TIM1->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency +TIM1->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% +TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable + +TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable +TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode +TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 +TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 +TIM8->PSC = 0; // Set prescaler to 0 +TIM8->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency +TIM8->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% +TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable + +// Set initial dead time value +TIM1->BDTR |= (uint32_t)(50 / 11.9); + + + return NULL; + + } @@ -823,20 +851,79 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); } -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 8PWM setting -// - hardware specific -void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, float dc_3a, float dc_3b, float dc_4a, float dc_4b, void* params) { - // transform duty cycle from [0,1] to [0,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE * dc_1a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE * dc_1b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE * dc_2a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE * dc_2b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE * dc_3a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _PWM_RANGE * dc_3b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[6], ((STM32DriverParams*)params)->channels[6], _PWM_RANGE * dc_4a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[7], ((STM32DriverParams*)params)->channels[7], _PWM_RANGE * dc_4b, _PWM_RESOLUTION); -} + + + +class EightPWMParams { +public: + int period; + TIM_HandleTypeDef tim1_handle; + TIM_HandleTypeDef tim8_handle; + + // Public members and methods go here +}; + +struct EightPWMConfig { + uint16_t freq; // PWM frequency in Hz + float duty_cycle_max; // Maximum duty cycle as a fraction of the period (0.0 to 1.0) +}; + + + // Scale duty cycles to the PWM period + class EightPWM { +public: + EightPWM(int period, TIM_HandleTypeDef tim1_handle, TIM_HandleTypeDef tim8_handle) + : period(period), tim1_handle(tim1_handle), tim8_handle(tim8_handle) {} + + void writeDutyCycle(float duty_cycle1A_h1, float duty_cycle1A_h2, float duty_cycle1B_h1, float duty_cycle1B_h2, + float duty_cycle2A_h1, float duty_cycle2A_h2, float duty_cycle2B_h1, float duty_cycle2B_h2) { + + // Scale duty cycles to the PWM period + uint16_t duty1A_h1 = (uint16_t)(duty_cycle1A_h1 * period); + uint16_t duty1A_h2 = (uint16_t)(duty_cycle1A_h2 * period); + uint16_t duty1B_h1 = (uint16_t)(duty_cycle1B_h1 * period); + uint16_t duty1B_h2 = (uint16_t)(duty_cycle1B_h2 * period); + uint16_t duty2A_h1 = (uint16_t)(duty_cycle2A_h1 * period); + uint16_t duty2A_h2 = (uint16_t)(duty_cycle2A_h2 * period); + uint16_t duty2B_h1 = (uint16_t)(duty_cycle2B_h1 * period); + uint16_t duty2B_h2 = (uint16_t)(duty_cycle2B_h2 * period); + + // Set duty cycles for TIM1 channels + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1, duty1A_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1N, duty1A_h2); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2, duty1B_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2N, duty1B_h2); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3, duty2A_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3N, duty2A_h2); + + // Set duty cycles for TIM8 channels + __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2, duty2B_h1); + __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2N, duty2B_h2); + + // Enable PWM outputs + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH1); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH1); + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH1N); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH1N); + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH2); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH2N); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH3); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH3); + HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH3N); + HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH3N); + HAL_TIM_PWM_Start(&tim8_handle, LL_TIM_CHANNEL_CH2); + HAL_TIMEx_PWMN_Start(&tim8_handle, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_Start(&tim8_handle, LL_TIM_CHANNEL_CH2N); + HAL_TIMEx_PWMN_Start(&tim8_handle, LL_TIM_CHANNEL_CH2N); + } + +private: + int period; + TIM_HandleTypeDef tim1_handle; + TIM_HandleTypeDef tim8_handle; +}; // Configuring PWM frequency, resolution and alignment From d4b86b95470101be707310767ec842770dbf8017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 02:00:40 +0100 Subject: [PATCH 16/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 0779f37d..5424794c 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -691,13 +691,13 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - // Configure TIM8 pins for alternate function mode with push-pull output - GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_0; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + // Configure TIM8 pins for alternate function mode with push-pull output + GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_0; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); @@ -774,26 +774,26 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) -TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable -TIM1->CR1 &= ~TIM_CR1_DIR; // Up counting mode -TIM1->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 -TIM1->CCER |= TIM_CCER_CC1E; // Enable output on OC1 -TIM1->PSC = 0; // Set prescaler to 0 -TIM1->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency -TIM1->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% -TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable + TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable + TIM1->CR1 &= ~TIM_CR1_DIR; // Up counting mode + TIM1->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 + TIM1->CCER |= TIM_CCER_CC1E; // Enable output on OC1 + TIM1->PSC = 0; // Set prescaler to 0 + TIM1->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency + TIM1->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% + TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable -TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable -TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode -TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 -TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 -TIM8->PSC = 0; // Set prescaler to 0 -TIM8->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency -TIM8->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% -TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable + TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable + TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode + TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 + TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 + TIM8->PSC = 0; // Set prescaler to 0 + TIM8->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency + TIM8->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% + TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable -// Set initial dead time value -TIM1->BDTR |= (uint32_t)(50 / 11.9); + // Set initial dead time value + TIM1->BDTR |= (uint32_t)(50 / 11.9); return NULL; From 16f9fc213bce95334121e75e58e924dfd2d479ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 13:02:04 +0100 Subject: [PATCH 17/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 5424794c..42e1d9e6 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -681,23 +681,29 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - // Configure GPIO pins for STSPIN32G4 internal FET driver - - __HAL_RCC_GPIOE_CLK_ENABLE(); // enable GPIO clock - GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_13 | GPIO_PIN_12; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // alternate function mode with push-pull output + // Configure PE8, PE9, PE10, PE11, PE12, PE13 as TIM1 channels + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; - HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); + HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - // Configure TIM8 pins for alternate function mode with push-pull output - GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_0; + // Configure PA14 and PB0 as TIM8 channels + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + GPIO_InitStruct.Pin = GPIO_PIN_14; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.Pin = GPIO_PIN_0; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); From 41834d873a774fd1f4dd1c25678b3a6ae430851b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 13:05:51 +0100 Subject: [PATCH 18/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 42e1d9e6..5882ae80 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -778,31 +778,37 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); - - + // Configure TIM1 for PWM output TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable TIM1->CR1 &= ~TIM_CR1_DIR; // Up counting mode TIM1->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 - TIM1->CCER |= TIM_CCER_CC1E; // Enable output on OC1 + TIM1->CCMR2 |= TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2; // PWM mode 1 on OC3 and OC4 + TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC3E | TIM_CCER_CC4E; // Enable output on OC1, OC3, and OC4 TIM1->PSC = 0; // Set prescaler to 0 TIM1->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency - TIM1->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% + TIM1->CCR1 = 0; // Set duty cycle to 0% + TIM1->CCR3 = 0; // Set duty cycle to 0% + TIM1->CCR4 = 0; // Set duty cycle to 0% TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable + // Configure TIM8 for PWM output TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 TIM8->PSC = 0; // Set prescaler to 0 TIM8->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency - TIM8->CCR1 = (SystemCoreClock / (38000 * 2)) / 2; // Set duty cycle to 50% + TIM8->CCR1 = 0; // Set duty cycle to 0% TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable - // Set initial dead time value - TIM1->BDTR |= (uint32_t)(50 / 11.9); + // Start TIM1 and TIM8 PWM outputs + TIM1->EGR |= TIM_EGR_UG; // Generate update event to load new settings + TIM8->EGR |= TIM_EGR_UG; // Generate update event to load new settings + TIM1->CR1 |= TIM_CR1_CEN; // Enable TIM1 counter + TIM8->CR1 |= TIM_CR1_CEN; // Enable TIM8 counter - - return NULL; + + return NULL; } From 73bc24b26984d194c4979bab002691cf9683f466 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 13:20:39 +0100 Subject: [PATCH 19/29] Update stm32_mcu.cpp --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 5882ae80..526e7a4b 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -815,7 +815,19 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) +/* + +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. + +*/ From 1bf6033a91835f152d5b84958af9c87cead2548e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 13:48:04 +0100 Subject: [PATCH 20/29] Update stm32_mcu.cpp --- 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 526e7a4b..8d71724e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -713,7 +713,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) htim1.Instance = TIM1; htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 255; + htim1.Init.Period = 65536; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; HAL_TIM_PWM_Init(&htim1); @@ -721,7 +721,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) htim8.Instance = TIM8; htim8.Init.Prescaler = 0; htim8.Init.CounterMode = TIM_COUNTERMODE_UP; - htim8.Init.Period = 255; + htim8.Init.Period = 65536; htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim8.Init.RepetitionCounter = 0; HAL_TIM_PWM_Init(&htim8); From 98e876cae5c03ceb97e974123ed8b28dda0980b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 14:14:33 +0100 Subject: [PATCH 21/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 8d71724e..a8eca13b 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -768,15 +768,19 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2); HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); - // Start TIM1 and TIM8 PWM outputs - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1N); - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2N); - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3); - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3N); - HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2N); + // Enable PWM outputs + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); + HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH1N); + + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2); + HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH2N); + + HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3); + HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH3N); + + HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); + HAL_TIMEx_PWMN_Start(&htim8, LL_TIM_CHANNEL_CH2N); + // Configure TIM1 for PWM output TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable From 220a0ca76c70692771fbea11aad171ac92f8ceab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 14:54:23 +0100 Subject: [PATCH 22/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 103 ++++++++++-------- 1 file changed, 55 insertions(+), 48 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index a8eca13b..f4f78815 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -768,7 +768,8 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2); HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); - // Enable PWM outputs + + // Enable PWM outputs HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH1N); @@ -780,7 +781,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); HAL_TIMEx_PWMN_Start(&htim8, LL_TIM_CHANNEL_CH2N); - + // Configure TIM1 for PWM output TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable @@ -817,8 +818,6 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) } - - /* 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), @@ -834,6 +833,8 @@ the code next to each other to ensure that the timers are properly initialized a */ + + @@ -903,56 +904,62 @@ struct EightPWMConfig { EightPWM(int period, TIM_HandleTypeDef tim1_handle, TIM_HandleTypeDef tim8_handle) : period(period), tim1_handle(tim1_handle), tim8_handle(tim8_handle) {} - void writeDutyCycle(float duty_cycle1A_h1, float duty_cycle1A_h2, float duty_cycle1B_h1, float duty_cycle1B_h2, - float duty_cycle2A_h1, float duty_cycle2A_h2, float duty_cycle2B_h1, float duty_cycle2B_h2) { - - // Scale duty cycles to the PWM period - uint16_t duty1A_h1 = (uint16_t)(duty_cycle1A_h1 * period); - uint16_t duty1A_h2 = (uint16_t)(duty_cycle1A_h2 * period); - uint16_t duty1B_h1 = (uint16_t)(duty_cycle1B_h1 * period); - uint16_t duty1B_h2 = (uint16_t)(duty_cycle1B_h2 * period); - uint16_t duty2A_h1 = (uint16_t)(duty_cycle2A_h1 * period); - uint16_t duty2A_h2 = (uint16_t)(duty_cycle2A_h2 * period); - uint16_t duty2B_h1 = (uint16_t)(duty_cycle2B_h1 * period); - uint16_t duty2B_h2 = (uint16_t)(duty_cycle2B_h2 * period); - - // Set duty cycles for TIM1 channels - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1, duty1A_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1N, duty1A_h2); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2, duty1B_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2N, duty1B_h2); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3, duty2A_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3N, duty2A_h2); - - // Set duty cycles for TIM8 channels - __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2, duty2B_h1); - __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2N, duty2B_h2); - - // Enable PWM outputs - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH1); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH1); - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH1N); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH1N); - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH2); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH2N); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH2N); - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH3); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH3); - HAL_TIM_PWM_Start(&tim1_handle, LL_TIM_CHANNEL_CH3N); - HAL_TIMEx_PWMN_Start(&tim1_handle, LL_TIM_CHANNEL_CH3N); - HAL_TIM_PWM_Start(&tim8_handle, LL_TIM_CHANNEL_CH2); - HAL_TIMEx_PWMN_Start(&tim8_handle, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_Start(&tim8_handle, LL_TIM_CHANNEL_CH2N); - HAL_TIMEx_PWMN_Start(&tim8_handle, LL_TIM_CHANNEL_CH2N); - } + void writeDutyCycle(float duty_cycle1_h1, float duty_cycle1_h2, float duty_cycle2_h1, float duty_cycle2_h2, + float duty_cycle3_h1, float duty_cycle3_h2, float duty_cycle4_h1, float duty_cycle4_h2) { + + // Scale duty cycles to the PWM period + uint16_t duty1_h1 = (uint16_t)(duty_cycle1_h1 * period); + uint16_t duty1_h2 = (uint16_t)(duty_cycle1_h2 * period); + uint16_t duty2_h1 = (uint16_t)(duty_cycle2_h1 * period); + uint16_t duty2_h2 = (uint16_t)(duty_cycle2_h2 * period); + uint16_t duty3_h1 = (uint16_t)(duty_cycle3_h1 * period); + uint16_t duty3_h2 = (uint16_t)(duty_cycle3_h2 * period); + uint16_t duty4_h1 = (uint16_t)(duty_cycle4_h1 * period); + uint16_t duty4_h2 = (uint16_t)(duty_cycle4_h2 * period); + + // Set duty cycles for half-bridge driver 1 + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1, duty1_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1N, duty1_h2); + + // Set duty cycles for half-bridge driver 2 + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2, duty2_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2N, duty2_h2); + + // Set duty cycles for half-bridge driver 3 + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3, duty3_h1); + __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3N, duty3_h2); + + // Set duty cycles for half-bridge driver 4 + __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2, duty4_h1); + __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2N, duty4_h2); +} private: int period; TIM_HandleTypeDef tim1_handle; TIM_HandleTypeDef tim8_handle; -}; +/* + +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 From 4d1ee20cb6074b49d141bede2569f9b95b2fe2b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Thu, 23 Mar 2023 21:48:04 +0100 Subject: [PATCH 23/29] Complimentary outputs.. --- .../hardware_specific/stm32/stm32_mcu.cpp | 96 +++++++++---------- 1 file changed, 46 insertions(+), 50 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index f4f78815..6c48f672 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -686,7 +686,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM1; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); // Configure PA14 and PB0 as TIM8 channels @@ -696,18 +696,18 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + GPIO_InitStruct.Alternate = GPIO_AF5_TIM8; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_0; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; + GPIO_InitStruct.Alternate = GPIO_AF4_TIM8; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - // Set TIM1 and TIM8 dead time values to 50 ns + //Set initial values TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; htim1.Instance = TIM1; @@ -730,6 +730,8 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + + // Set TIM1 dead time values to 50 ns sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; @@ -750,23 +752,30 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) sBreakDeadTimeConfigTIM8.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfigTIM8); + // Configure TIM1 channels 1-6 for PWM output + TIM_OC_InitTypeDef sConfigOC1 = {0}; + sConfigOC1.OCMode = TIM_OCMODE_PWM1; + sConfigOC1.Pulse = 0; + sConfigOC1.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC1.OCFastMode = TIM_OCFAST_DISABLE; + + TIM_OC_InitTypeDef sConfigOC2 = {0}; + sConfigOC2.OCMode = TIM_OCMODE_PWM2; + sConfigOC2.Pulse = 0; + sConfigOC2.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC2.OCFastMode = TIM_OCFAST_DISABLE; - // Configure TIM1 channels 1-6 for PWM output - TIM_OC_InitTypeDef sConfigOC = {0}; - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH1); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH1N); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH2N); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH3); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, LL_TIM_CHANNEL_CH3N); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH1); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH1N); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH2N); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH3); + HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH3N); + + // Configure PWM output on TIM8 channel 1 and additional channel + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC1, LL_TIM_CHANNEL_CH2); + HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC2, LL_TIM_CHANNEL_CH2N); - // Configure PWM output on TIM8 channel 1 and additional channel - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, LL_TIM_CHANNEL_CH2N); // Enable PWM outputs @@ -883,28 +892,15 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo -class EightPWMParams { -public: - int period; - TIM_HandleTypeDef tim1_handle; - TIM_HandleTypeDef tim8_handle; - - // Public members and methods go here -}; -struct EightPWMConfig { - uint16_t freq; // PWM frequency in Hz - float duty_cycle_max; // Maximum duty cycle as a fraction of the period (0.0 to 1.0) -}; - // Scale duty cycles to the PWM period - class EightPWM { -public: - EightPWM(int period, TIM_HandleTypeDef tim1_handle, TIM_HandleTypeDef tim8_handle) - : period(period), tim1_handle(tim1_handle), tim8_handle(tim8_handle) {} - void writeDutyCycle(float duty_cycle1_h1, float duty_cycle1_h2, float duty_cycle2_h1, float duty_cycle2_h2, + + int period; + + + void _writeDutyCycle8PWM(float duty_cycle1_h1, float duty_cycle1_h2, float duty_cycle2_h1, float duty_cycle2_h2, float duty_cycle3_h1, float duty_cycle3_h2, float duty_cycle4_h1, float duty_cycle4_h2) { // Scale duty cycles to the PWM period @@ -918,26 +914,26 @@ struct EightPWMConfig { uint16_t duty4_h2 = (uint16_t)(duty_cycle4_h2 * period); // Set duty cycles for half-bridge driver 1 - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1, duty1_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH1N, duty1_h2); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH1, duty1_h1); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH1N, duty1_h2); // Set duty cycles for half-bridge driver 2 - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2, duty2_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH2N, duty2_h2); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH2, duty2_h1); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH2N, duty2_h2); // Set duty cycles for half-bridge driver 3 - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3, duty3_h1); - __HAL_TIM_SET_COMPARE(&tim1_handle, LL_TIM_CHANNEL_CH3N, duty3_h2); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH3, duty3_h1); + __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH3N, duty3_h2); // Set duty cycles for half-bridge driver 4 - __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2, duty4_h1); - __HAL_TIM_SET_COMPARE(&tim8_handle, LL_TIM_CHANNEL_CH2N, duty4_h2); + __HAL_TIM_SET_COMPARE(&htim8, LL_TIM_CHANNEL_CH2, duty4_h1); + __HAL_TIM_SET_COMPARE(&htim8, LL_TIM_CHANNEL_CH2N, duty4_h2); + + } -private: - int period; - TIM_HandleTypeDef tim1_handle; - TIM_HandleTypeDef tim8_handle; + + /* @@ -959,7 +955,7 @@ simply call "__HAL_TIM_SET_COMPARE" to update the compare value and the timer wi */ -}; + // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting From aa2f9a8f00b1fb0de1c4d3d5e03c94ffefea2472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Fri, 24 Mar 2023 09:48:50 +0100 Subject: [PATCH 24/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 127 ++++++++++++++---- 1 file changed, 98 insertions(+), 29 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 6c48f672..3f885e08 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -712,23 +712,31 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; htim1.Instance = TIM1; htim1.Init.Prescaler = 0; - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 65536; + htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + htim1.Init.Period = 4359; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; + htim1.Init.RepetitionCounter = 1; HAL_TIM_PWM_Init(&htim1); htim8.Instance = TIM8; htim8.Init.Prescaler = 0; - htim8.Init.CounterMode = TIM_COUNTERMODE_UP; - htim8.Init.Period = 65536; + htim8.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + htim8.Init.Period = 4359; htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim8.Init.RepetitionCounter = 0; + htim8.Init.RepetitionCounter = 1; HAL_TIM_PWM_Init(&htim8); sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("Timer TIM1 Synced OK!");} + + if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("Timer TIM8 Synced OK!");} + + // Set TIM1 dead time values to 50 ns @@ -739,7 +747,9 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); + if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1 DeadTime Set");} // Set TIM8 dead time values to 50 ns TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfigTIM8 = {0}; @@ -750,9 +760,11 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) sBreakDeadTimeConfigTIM8.BreakState = TIM_BREAK_DISABLE; sBreakDeadTimeConfigTIM8.BreakPolarity = TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfigTIM8.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfigTIM8); + if (HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfigTIM8)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8 DeadTime Set");} - // Configure TIM1 channels 1-6 for PWM output + // Configure TIM1 channels 1-6 for PWM output TIM_OC_InitTypeDef sConfigOC1 = {0}; sConfigOC1.OCMode = TIM_OCMODE_PWM1; sConfigOC1.Pulse = 0; @@ -762,34 +774,91 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) TIM_OC_InitTypeDef sConfigOC2 = {0}; sConfigOC2.OCMode = TIM_OCMODE_PWM2; sConfigOC2.Pulse = 0; - sConfigOC2.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC2.OCPolarity = TIM_OCPOLARITY_LOW; sConfigOC2.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH1); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH1N); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH2N); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH3); - HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH3N); + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH1)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH1 Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH1N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH1N Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH2)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH2 Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH2N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH2N Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH3)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH3 Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH3N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH3N Config OK");} + // Configure PWM output on TIM8 channel 1 and additional channel - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC1, LL_TIM_CHANNEL_CH2); - HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC2, LL_TIM_CHANNEL_CH2N); + if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC1, LL_TIM_CHANNEL_CH2)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2 Config OK");} + + if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC2, LL_TIM_CHANNEL_CH2N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2N Config OK");} + + +/* + +For TIM1, you have CH1 and CH1N as complementary outputs, CH2 and CH2N as complementary outputs, +and CH3 and CH3N as complementary outputs. + +For TIM8, you have CH2 and CH2N as complementary outputs. +Complementary outputs are a pair of signals where one is the inverse of the other, +and they are used for driving push-pull outputs such as MOSFETs. + +*/ // Enable PWM outputs - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1); - HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH1N); + if( HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH1 Start OK");} + + if (HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH1N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH1N Start OK");} + + if (HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH2 Start OK");} + + if (HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH2N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH2N Start OK");} + + if(HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH3 Start OK");} + + if( HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH3N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("CH3N Start OK");} + - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2); - HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH2N); + if (HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2 Start OK");} - HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3); - HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH3N); + if (HAL_TIMEx_PWMN_Start(&htim8, LL_TIM_CHANNEL_CH2N)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2N Start OK");} - HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2); - HAL_TIMEx_PWMN_Start(&htim8, LL_TIM_CHANNEL_CH2N); // Configure TIM1 for PWM output @@ -799,7 +868,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) TIM1->CCMR2 |= TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2; // PWM mode 1 on OC3 and OC4 TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC3E | TIM_CCER_CC4E; // Enable output on OC1, OC3, and OC4 TIM1->PSC = 0; // Set prescaler to 0 - TIM1->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency + TIM1->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency TIM1->CCR1 = 0; // Set duty cycle to 0% TIM1->CCR3 = 0; // Set duty cycle to 0% TIM1->CCR4 = 0; // Set duty cycle to 0% @@ -811,7 +880,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 TIM8->PSC = 0; // Set prescaler to 0 - TIM8->ARR = (SystemCoreClock / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency + TIM8->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency TIM8->CCR1 = 0; // Set duty cycle to 0% TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable From deba1dc1581c6bb146599b3f1c0f0f25a88a9633 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Fri, 24 Mar 2023 10:10:55 +0100 Subject: [PATCH 25/29] TIM1 and TIM8 INIT --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 3f885e08..56fa7b60 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -716,7 +716,9 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) htim1.Init.Period = 4359; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 1; - HAL_TIM_PWM_Init(&htim1); + if (HAL_TIM_PWM_Init(&htim1)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1 INIT OK!");} htim8.Instance = TIM8; htim8.Init.Prescaler = 0; @@ -724,7 +726,9 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) htim8.Init.Period = 4359; htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim8.Init.RepetitionCounter = 1; - HAL_TIM_PWM_Init(&htim8); + if (HAL_TIM_PWM_Init(&htim8)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8 INIT OK!");} sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; From f39e628fc3a791d24e4d3166080d232b4287c962 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Fri, 24 Mar 2023 10:49:52 +0100 Subject: [PATCH 26/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 56fa7b60..f16d2fa5 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -680,7 +680,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) // Enable clock for TIM1 and TIM8 __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - + __HAL_RCC_GPIOE_CLK_ENABLE(); // Configure PE8, PE9, PE10, PE11, PE12, PE13 as TIM1 channels GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -730,6 +730,17 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; }else{Serial.println("TIM8 INIT OK!");} + /* + *************************************************** + Yes, those values should produce a 38kHz PWM signal. + what should the value be for 50Khz PWM signal? + The value for a 50kHz PWM signal would be: htim8.Init.Period = 3279; + And 50Khz for this: TIM1->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency is egual + No, the equation should be changed to calculate the auto-reload value for a 50kHz frequency. + The equation would be: TIM1->ARR = (168000000 / (50000 * 2)) - 1; // Set auto-reload value for 50kHz frequency + ******************************************************************************************************************* + */ + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig)!= HAL_OK) @@ -874,6 +885,7 @@ and they are used for driving push-pull outputs such as MOSFETs. TIM1->PSC = 0; // Set prescaler to 0 TIM1->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency TIM1->CCR1 = 0; // Set duty cycle to 0% + TIM1->CCR2 = 0; // Set duty cycle to 0% TIM1->CCR3 = 0; // Set duty cycle to 0% TIM1->CCR4 = 0; // Set duty cycle to 0% TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable @@ -886,6 +898,9 @@ and they are used for driving push-pull outputs such as MOSFETs. TIM8->PSC = 0; // Set prescaler to 0 TIM8->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency TIM8->CCR1 = 0; // Set duty cycle to 0% + TIM8->CCR2 = 0; // Set duty cycle to 0% + TIM8->CCR3 = 0; // Set duty cycle to 0% + TIM8->CCR4 = 0; // Set duty cycle to 0% TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable // Start TIM1 and TIM8 PWM outputs From 2456d3bbbb851e67cd2259931ddb529841b5577c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Fri, 24 Mar 2023 17:23:05 +0100 Subject: [PATCH 27/29] Set all outputs LOW --- .../hardware_specific/stm32/stm32_mcu.cpp | 40 ++++++++++++++++--- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index f16d2fa5..104b59e4 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -680,7 +680,7 @@ void* _configure8PWM(long pwm_frequency, float dead_zone) // Enable clock for TIM1 and TIM8 __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); - __HAL_RCC_GPIOE_CLK_ENABLE(); + // Configure PE8, PE9, PE10, PE11, PE12, PE13 as TIM1 channels GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -888,13 +888,27 @@ and they are used for driving push-pull outputs such as MOSFETs. TIM1->CCR2 = 0; // Set duty cycle to 0% TIM1->CCR3 = 0; // Set duty cycle to 0% TIM1->CCR4 = 0; // Set duty cycle to 0% - TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable - // Configure TIM8 for PWM output + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH8); // Clear alternate function for PE8 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH8_Pos); // Set alternate function for PE8 + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH9); // Clear alternate function for PE9 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH9_Pos); // Set alternate function for PE9 + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH10); // Clear alternate function for PE10 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH10_Pos); // Set alternate function for PE10 + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH11); // Clear alternate function for PE11 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH11_Pos); // Set alternate function for PE11 + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH12); // Clear alternate function for PE12 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH12_Pos); // Set alternate function for PE12 + GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH13); // Clear alternate function for PE13 + GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH13_Pos); // Set alternate function for PE13 + TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable +// Configure CH2 and CH2N on TIM8 TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode - TIM8->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 - TIM8->CCER |= TIM_CCER_CC1E; // Enable output on OC1 + TIM8->CR1 &= ~TIM_CR1_CMS; // Set counter mode to center-aligned + TIM8->CR1 |= TIM_CR1_CMS_0 | TIM_CR1_CMS_1; // Set counter mode to center-aligned 3 + TIM8->CCMR1 |= TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2; // PWM mode 1 on OC2 + TIM8->CCER |= TIM_CCER_CC2E; // Enable output on OC2 TIM8->PSC = 0; // Set prescaler to 0 TIM8->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency TIM8->CCR1 = 0; // Set duty cycle to 0% @@ -903,12 +917,28 @@ and they are used for driving push-pull outputs such as MOSFETs. TIM8->CCR4 = 0; // Set duty cycle to 0% TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable + GPIOA->AFR[1] &= ~(GPIO_AFRH_AFRH2); // Clear alternate function for PA14 + GPIOA->AFR[1] |= (0x8 << GPIO_AFRH_AFRH2_Pos); // Set alternate function for PA14 + GPIOB->AFR[0] &= ~(GPIO_AFRL_AFRL0); // Clear alternate function for PB0 + GPIOB->AFR[0] |= (0x8 << GPIO_AFRL_AFRL0_Pos); // Set alternate function for PB0 + TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable + // Start TIM1 and TIM8 PWM outputs TIM1->EGR |= TIM_EGR_UG; // Generate update event to load new settings TIM8->EGR |= TIM_EGR_UG; // Generate update event to load new settings TIM1->CR1 |= TIM_CR1_CEN; // Enable TIM1 counter TIM8->CR1 |= TIM_CR1_CEN; // Enable TIM8 counter + // Setting ALL pins to LOW + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_14, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + return NULL; From fa0ddf3f8c48f287efd7035ca9d162ba530ca3ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Fri, 24 Mar 2023 20:53:52 +0100 Subject: [PATCH 28/29] Update stm32_mcu.cpp --- .../hardware_specific/stm32/stm32_mcu.cpp | 105 ++++++------------ 1 file changed, 34 insertions(+), 71 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 104b59e4..3fde0b26 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -876,68 +876,41 @@ and they are used for driving push-pull outputs such as MOSFETs. - // Configure TIM1 for PWM output - TIM1->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable - TIM1->CR1 &= ~TIM_CR1_DIR; // Up counting mode - TIM1->CCMR1 |= TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2; // PWM mode 1 on OC1 - TIM1->CCMR2 |= TIM_CCMR2_OC3PE | TIM_CCMR2_OC4PE | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2; // PWM mode 1 on OC3 and OC4 - TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC3E | TIM_CCER_CC4E; // Enable output on OC1, OC3, and OC4 - TIM1->PSC = 0; // Set prescaler to 0 - TIM1->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency - TIM1->CCR1 = 0; // Set duty cycle to 0% - TIM1->CCR2 = 0; // Set duty cycle to 0% - TIM1->CCR3 = 0; // Set duty cycle to 0% - TIM1->CCR4 = 0; // Set duty cycle to 0% - - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH8); // Clear alternate function for PE8 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH8_Pos); // Set alternate function for PE8 - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH9); // Clear alternate function for PE9 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH9_Pos); // Set alternate function for PE9 - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH10); // Clear alternate function for PE10 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH10_Pos); // Set alternate function for PE10 - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH11); // Clear alternate function for PE11 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH11_Pos); // Set alternate function for PE11 - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH12); // Clear alternate function for PE12 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH12_Pos); // Set alternate function for PE12 - GPIOE->AFR[1] &= ~(GPIO_AFRH_AFRH13); // Clear alternate function for PE13 - GPIOE->AFR[1] |= (0x2 << GPIO_AFRH_AFRH13_Pos); // Set alternate function for PE13 - TIM1->BDTR |= TIM_BDTR_MOE; // Main output enable -// Configure CH2 and CH2N on TIM8 - TIM8->CR1 |= TIM_CR1_ARPE; // Auto-reload preload enable - TIM8->CR1 &= ~TIM_CR1_DIR; // Up counting mode - TIM8->CR1 &= ~TIM_CR1_CMS; // Set counter mode to center-aligned - TIM8->CR1 |= TIM_CR1_CMS_0 | TIM_CR1_CMS_1; // Set counter mode to center-aligned 3 - TIM8->CCMR1 |= TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2; // PWM mode 1 on OC2 - TIM8->CCER |= TIM_CCER_CC2E; // Enable output on OC2 - TIM8->PSC = 0; // Set prescaler to 0 - TIM8->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency - TIM8->CCR1 = 0; // Set duty cycle to 0% - TIM8->CCR2 = 0; // Set duty cycle to 0% - TIM8->CCR3 = 0; // Set duty cycle to 0% - TIM8->CCR4 = 0; // Set duty cycle to 0% - TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable - - GPIOA->AFR[1] &= ~(GPIO_AFRH_AFRH2); // Clear alternate function for PA14 - GPIOA->AFR[1] |= (0x8 << GPIO_AFRH_AFRH2_Pos); // Set alternate function for PA14 - GPIOB->AFR[0] &= ~(GPIO_AFRL_AFRL0); // Clear alternate function for PB0 - GPIOB->AFR[0] |= (0x8 << GPIO_AFRL_AFRL0_Pos); // Set alternate function for PB0 - TIM8->BDTR |= TIM_BDTR_MOE; // Main output enable - - // Start TIM1 and TIM8 PWM outputs - TIM1->EGR |= TIM_EGR_UG; // Generate update event to load new settings - TIM8->EGR |= TIM_EGR_UG; // Generate update event to load new settings - TIM1->CR1 |= TIM_CR1_CEN; // Enable TIM1 counter - TIM8->CR1 |= TIM_CR1_CEN; // Enable TIM8 counter + // Setting ALL pins to LOW - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOA, GPIO_PIN_14, GPIO_PIN_RESET); - HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH1 READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH1N READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH2 READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH2N READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH3 READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM1_CH3N READY - LOW OK");} + + if (HAL_GPIO_WritePin(GPIOA, GPIO_PIN_14, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2 READY - LOW OK");} + + + if (HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET)!= HAL_OK) + {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + }else{Serial.println("TIM8_CH2N READY - LOW OK");} return NULL; @@ -961,11 +934,6 @@ the code next to each other to ensure that the timers are properly initialized a - - - - - // function setting the pwm duty cycle to the hardware // - DC motor - 1PWM setting // - hardware speciffic @@ -1010,12 +978,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo - - - - - - int period; + int period = 4359; void _writeDutyCycle8PWM(float duty_cycle1_h1, float duty_cycle1_h2, float duty_cycle2_h1, float duty_cycle2_h2, From fe198f179dd3c87bbee43d09fa78a2cc29f5df1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan-Antonio=20S=C3=B8ren=20E=2E=20Pedersen?= Date: Sat, 30 Sep 2023 16:59:06 +0200 Subject: [PATCH 29/29] Dev. Changes Field-Stack Work --- src/SimpleFOC.h | 7 +- src/StepperMotor.cpp | 106 ++++- src/StepperMotor.h | 13 +- src/common/base_classes/FOCMotor.h | 3 + src/common/base_classes/Sensor.cpp | 16 +- src/common/base_classes/StepperDriver.h | 6 + .../hardware_specific/rp2040/rp2040_mcu.cpp | 1 - .../stm32/stm32l4/stm32l4_utils.cpp | 2 +- src/drivers/BLDCDriver6PWM.cpp | 8 +- src/drivers/BLDCDriver6PWM.h | 1 + src/drivers/StepperDriver8PWM.cpp | 127 +++-- src/drivers/StepperDriver8PWM.h | 16 +- src/drivers/hardware_api.h | 9 +- .../hardware_specific/stm32/stm32_mcu.cpp | 438 +++++------------- 14 files changed, 343 insertions(+), 410 deletions(-) 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 33148e7b..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,8 +371,18 @@ 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 + //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 @@ -387,8 +401,8 @@ int32_t float_to_q31(float input) { int32_t q31_value; float value_f32_sine = 0; float value_f32_cosine = 0; -float cordic_cosine = 0.0f; -float cordic_sine = 0.0f; +q31_t cordic_cosine; +q31_t cordic_sine; float wrap_to_1(float x) { while (x > 1.0f) { @@ -404,38 +418,84 @@ float wrap_to_1(float x) { /// @param Uq /// @param Ud /// @param angle_el -void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) { - // Sinusoidal PWM modulation - // Inverse Park transformation - // WHO U GONNA CALL? CORDIC -> +#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)); -q31_value = float_to_q31(angle_el / (2.0f * pi)); + /* Write angle and start CORDIC execution */ -CORDIC->WDATA = q31_value; + // 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; -if (angle_el < 0){ value_f32_sine = wrap_to_1(value_f32_sine); -value_f32_sine = value_f32_sine * -1; -} - -if (angle_el > 0){ -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; + //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); + + + } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 94e002a2..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 @@ -93,10 +95,15 @@ class StepperMotor: public FOCMotor void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override; - private: + + 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(); @@ -117,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 d8aaa45f..3c0d8de7 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -142,6 +142,8 @@ class FOCMotor 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(); @@ -162,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 4dd7203c..2a19e238 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -19,6 +19,12 @@ 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 * 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 index 1ec2b751..6d33e3b9 100644 --- a/src/drivers/StepperDriver8PWM.cpp +++ b/src/drivers/StepperDriver8PWM.cpp @@ -28,13 +28,35 @@ StepperDriver8PWM::StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int pinMode(enable_pin2, OUTPUT); disable(); - // dead zone initial - 2% + // 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); +} @@ -43,60 +65,77 @@ StepperDriver8PWM::StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int // 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); + + + // 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; -} + + + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; + -// Set voltage to the pwm pin for 8PWM control -void StepperDriver8PWM::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A_h1(0.0f),duty_cycle1A_h2(0.0f),duty_cycle1B_h1(0.0f),duty_cycle1B_h2(0.0f); - float duty_cycle2A_h1(0.0f),duty_cycle2A_h2(0.0f),duty_cycle2B_h1(0.0f),duty_cycle2B_h2(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_cycle1B_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); - duty_cycle1B_h2 = 0.0f; // set second half-bridge duty cycle to 0 - } - else { - duty_cycle1A_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); - duty_cycle1A_h2 = 0.0f; // set second half-bridge duty cycle to 0 - } - - if( Ubeta > 0 ) { - duty_cycle2B_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); - duty_cycle2B_h2 = 0.0f; // set second half-bridge duty cycle to 0 - } - else { - duty_cycle2A_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); - duty_cycle2A_h2 = 0.0f; // set second half-bridge duty cycle to 0 - } - - // write to hardware - _writeDutyCycle8PWM(duty_cycle1A_h1, duty_cycle1A_h2, duty_cycle1B_h1, duty_cycle1B_h2, - duty_cycle2A_h1, duty_cycle2A_h2, duty_cycle2B_h1, duty_cycle2B_h2, params); } + + + + // 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 index 34833f5d..fc70f590 100644 --- a/src/drivers/StepperDriver8PWM.h +++ b/src/drivers/StepperDriver8PWM.h @@ -38,15 +38,13 @@ class StepperDriver8PWM : public StepperDriver { 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; //!< phase 1A pwm pin number - int pwm1B; //!< phase 1B pwm pin number - int pwm2A; //!< phase 2A pwm pin number - int pwm2B; //!< phase 2B pwm pin number - int pwm3A; //!< phase 3A pwm pin number - int pwm3B; //!< phase 3B pwm pin number - int pwm4A; //!< phase 4A pwm pin number - int pwm4B; //!< phase 4B pwm pin number + 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 @@ -59,6 +57,8 @@ class StepperDriver8PWM : public StepperDriver { * @param Ub phase B voltage */ void setPwm(float Ua, float Ub) override; + + private: }; diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index b9065293..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 @@ -131,8 +131,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons * * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B, - const int pin3A, const int pin3B, const int pin4A, const int pin4B); +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); /** @@ -214,7 +213,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ *@param params the driver parameters */ -void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_1c, float dc_1d, float dc_2a, float dc_2b, float dc_2c, float dc_2d, void* params); +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 3fde0b26..4aa8ce98 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -255,6 +255,8 @@ 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; @@ -265,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); @@ -334,7 +345,7 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi } -STM32DriverParams* _initHardware8PWMInterface(long PWM_freq, float dead_zone, PinMap* pin1A, PinMap* pin1B, PinMap* pin2A, PinMap* pin2B, PinMap* pin3A, PinMap* pin3B, PinMap* pin4A, PinMap* pin4B) { +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 }, @@ -343,14 +354,18 @@ STM32DriverParams* _initHardware8PWMInterface(long PWM_freq, float dead_zone, Pi .interface_type = _HARDWARE_8PWM }; - if (_initHardware6PWMPair(PWM_freq, dead_zone, pin1A, pin1B, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + 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, pin2A, pin2B, params, 2) == 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, pin3A, pin3B, params, 4) == 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, pin4A, pin4B, params, 6) == 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; } @@ -402,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) @@ -416,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 @@ -502,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) @@ -531,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; @@ -657,266 +674,10 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in } -#include "stm32g4xx_hal.h" -#include "stm32g4xx_hal_gpio.h" -#include "stm32g4xx_hal_tim.h" -#include "stm32g4xx_hal_tim_ex.h" - - - - -// Declare timer handles for TIM1 and TIM8 -TIM_HandleTypeDef htim1; -TIM_HandleTypeDef htim8; - -// Function to configure PWM output on TIM1 channels 1-6 and TIM8 channel 1 -void* _configure8PWM(long pwm_frequency, float dead_zone) -{ - - - // GPIO pin initialization struct - GPIO_InitTypeDef GPIO_InitStruct = {0}; - - // Enable clock for TIM1 and TIM8 - __HAL_RCC_TIM1_CLK_ENABLE(); - __HAL_RCC_TIM8_CLK_ENABLE(); - - // Configure PE8, PE9, PE10, PE11, PE12, PE13 as TIM1 channels - GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF2_TIM1; - HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - - // Configure PA14 and PB0 as TIM8 channels - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - GPIO_InitStruct.Pin = GPIO_PIN_14; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF5_TIM8; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = GPIO_PIN_0; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF4_TIM8; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - - - //Set initial values - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - htim1.Instance = TIM1; - htim1.Init.Prescaler = 0; - htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - htim1.Init.Period = 4359; - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 1; - if (HAL_TIM_PWM_Init(&htim1)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1 INIT OK!");} - - htim8.Instance = TIM8; - htim8.Init.Prescaler = 0; - htim8.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - htim8.Init.Period = 4359; - htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim8.Init.RepetitionCounter = 1; - if (HAL_TIM_PWM_Init(&htim8)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8 INIT OK!");} - - /* - *************************************************** - Yes, those values should produce a 38kHz PWM signal. - what should the value be for 50Khz PWM signal? - The value for a 50kHz PWM signal would be: htim8.Init.Period = 3279; - And 50Khz for this: TIM1->ARR = (168000000 / (38000 * 2)) - 1; // Set auto-reload value for 38kHz frequency is egual - No, the equation should be changed to calculate the auto-reload value for a 50kHz frequency. - The equation would be: TIM1->ARR = (168000000 / (50000 * 2)) - 1; // Set auto-reload value for 50kHz frequency - ******************************************************************************************************************* - */ - - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("Timer TIM1 Synced OK!");} - - if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("Timer TIM8 Synced OK!");} - - - - - // Set TIM1 dead time values to 50 ns - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 50; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1 DeadTime Set");} - - // Set TIM8 dead time values to 50 ns - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfigTIM8 = {0}; - sBreakDeadTimeConfigTIM8.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfigTIM8.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfigTIM8.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfigTIM8.DeadTime = 50; - sBreakDeadTimeConfigTIM8.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfigTIM8.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfigTIM8.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfigTIM8)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8 DeadTime Set");} - - // Configure TIM1 channels 1-6 for PWM output - TIM_OC_InitTypeDef sConfigOC1 = {0}; - sConfigOC1.OCMode = TIM_OCMODE_PWM1; - sConfigOC1.Pulse = 0; - sConfigOC1.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC1.OCFastMode = TIM_OCFAST_DISABLE; - - TIM_OC_InitTypeDef sConfigOC2 = {0}; - sConfigOC2.OCMode = TIM_OCMODE_PWM2; - sConfigOC2.Pulse = 0; - sConfigOC2.OCPolarity = TIM_OCPOLARITY_LOW; - sConfigOC2.OCFastMode = TIM_OCFAST_DISABLE; - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH1)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH1 Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH1N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH1N Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH2)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH2 Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH2N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH2N Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC1, LL_TIM_CHANNEL_CH3)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH3 Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC2, LL_TIM_CHANNEL_CH3N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH3N Config OK");} - - - // Configure PWM output on TIM8 channel 1 and additional channel - if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC1, LL_TIM_CHANNEL_CH2)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2 Config OK");} - - if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC2, LL_TIM_CHANNEL_CH2N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2N Config OK");} - - -/* - -For TIM1, you have CH1 and CH1N as complementary outputs, CH2 and CH2N as complementary outputs, -and CH3 and CH3N as complementary outputs. - -For TIM8, you have CH2 and CH2N as complementary outputs. - -Complementary outputs are a pair of signals where one is the inverse of the other, -and they are used for driving push-pull outputs such as MOSFETs. -*/ - - - // Enable PWM outputs - if( HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH1)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH1 Start OK");} - - if (HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH1N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH1N Start OK");} - - if (HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH2)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH2 Start OK");} - - if (HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH2N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH2N Start OK");} - - if(HAL_TIM_PWM_Start(&htim1, LL_TIM_CHANNEL_CH3)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH3 Start OK");} - - if( HAL_TIMEx_PWMN_Start(&htim1, LL_TIM_CHANNEL_CH3N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("CH3N Start OK");} - - - if (HAL_TIM_PWM_Start(&htim8, LL_TIM_CHANNEL_CH2)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2 Start OK");} - - if (HAL_TIMEx_PWMN_Start(&htim8, LL_TIM_CHANNEL_CH2N)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2N Start OK");} - - - - - - // Setting ALL pins to LOW - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH1 READY - LOW OK");} - - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH1N READY - LOW OK");} - - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH2 READY - LOW OK");} - - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH2N READY - LOW OK");} - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH3 READY - LOW OK");} - if (HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM1_CH3N READY - LOW OK");} - if (HAL_GPIO_WritePin(GPIOA, GPIO_PIN_14, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2 READY - LOW OK");} - - - if (HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET)!= HAL_OK) - {return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - }else{Serial.println("TIM8_CH2N READY - LOW OK");} - - - return NULL; - - -} /* @@ -978,40 +739,22 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo - int period = 4359; + 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; + } +} - void _writeDutyCycle8PWM(float duty_cycle1_h1, float duty_cycle1_h2, float duty_cycle2_h1, float duty_cycle2_h2, - float duty_cycle3_h1, float duty_cycle3_h2, float duty_cycle4_h1, float duty_cycle4_h2) { - - // Scale duty cycles to the PWM period - uint16_t duty1_h1 = (uint16_t)(duty_cycle1_h1 * period); - uint16_t duty1_h2 = (uint16_t)(duty_cycle1_h2 * period); - uint16_t duty2_h1 = (uint16_t)(duty_cycle2_h1 * period); - uint16_t duty2_h2 = (uint16_t)(duty_cycle2_h2 * period); - uint16_t duty3_h1 = (uint16_t)(duty_cycle3_h1 * period); - uint16_t duty3_h2 = (uint16_t)(duty_cycle3_h2 * period); - uint16_t duty4_h1 = (uint16_t)(duty_cycle4_h1 * period); - uint16_t duty4_h2 = (uint16_t)(duty_cycle4_h2 * period); - - // Set duty cycles for half-bridge driver 1 - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH1, duty1_h1); - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH1N, duty1_h2); - - // Set duty cycles for half-bridge driver 2 - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH2, duty2_h1); - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH2N, duty2_h2); - - // Set duty cycles for half-bridge driver 3 - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH3, duty3_h1); - __HAL_TIM_SET_COMPARE(&htim1, LL_TIM_CHANNEL_CH3N, duty3_h2); - - // Set duty cycles for half-bridge driver 4 - __HAL_TIM_SET_COMPARE(&htim8, LL_TIM_CHANNEL_CH2, duty4_h1); - __HAL_TIM_SET_COMPARE(&htim8, LL_TIM_CHANNEL_CH2N, duty4_h2); - - -} + @@ -1047,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; @@ -1083,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; @@ -1161,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 @@ -1250,6 +1059,7 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { SimpleFOCDebug::print(" "); } SimpleFOCDebug::println("score: ", score); + delay(100); } #endif