diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 8ee1a727..11b7b945 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -25,6 +25,21 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -56,6 +71,14 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 49318c52..39d9bf38 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -17,6 +17,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -50,6 +64,18 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 9001fd90..1e2adb81 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -28,6 +28,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -59,6 +73,17 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index 55348e2b..2d34493e 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -53,6 +53,33 @@ void _setHighFrequency(const long freq, const int pin, const int channel){ + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + + + + + + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -123,6 +150,12 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in +void _writeDutyCycle1PWM(float dc_a, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 5bdfd5d2..0f8c9397 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -12,6 +12,22 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -60,6 +76,13 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 565a3c96..f7b58f80 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -95,6 +95,19 @@ void syncSlices() { } + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + syncSlices(); + return params; +} + + + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; @@ -165,6 +178,12 @@ void writeDutyCycle(float val, uint slice, uint chan) { +void _writeDutyCycle1PWM(float dc_a, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index a9191702..f9cd8467 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -14,6 +14,36 @@ #endif +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER +#define TCC3_CH1 NOT_ON_TIMER +#endif + +#ifndef TCC4_CH0 +#define TCC4_CH0 NOT_ON_TIMER +#define TCC4_CH1 NOT_ON_TIMER +#endif + + +#ifndef TC4_CH0 +#define TC4_CH0 NOT_ON_TIMER +#define TC4_CH1 NOT_ON_TIMER +#endif + +#ifndef TC5_CH0 +#define TC5_CH0 NOT_ON_TIMER +#define TC5_CH1 NOT_ON_TIMER +#endif + +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#define TC6_CH1 NOT_ON_TIMER +#endif + +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#define TC7_CH1 NOT_ON_TIMER +#endif @@ -109,7 +139,12 @@ struct wo_association WO_associations[] = { wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; +#ifndef TCC3_CC_NUM +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM }; +#else uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; +#endif + struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { for (int i=0;ipins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic @@ -68,6 +94,8 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic