From 7902e62401bfc113f046555d175543cf4491b0e1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 12 Oct 2021 18:40:41 +0200 Subject: [PATCH 001/474] make AREF pin optional in SAMD current sensing --- src/current_sense/hardware_specific/samd21_mcu.cpp | 9 ++++++--- src/current_sense/hardware_specific/samd21_mcu.h | 6 ++++-- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 8bb6d38c..7d742a2e 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -137,7 +137,8 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { void SAMDCurrentSenseADCDMA::initPins(){ - pinMode(pinAREF, INPUT); + if (pinAREF>=0) + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -169,8 +170,10 @@ void SAMDCurrentSenseADCDMA::initADC(){ //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default - ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; - // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + if (pinAREF>=0) + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + else + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 /* diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c0cec74a..e7d74426 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -3,7 +3,7 @@ #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H -// #define SIMPLEFOC_SAMD_DEBUG +#define SIMPLEFOC_SAMD_DEBUG #if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) #define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial #endif @@ -18,6 +18,8 @@ } dmacdescriptor ; +// AREF pin is 42 + class SAMDCurrentSenseADCDMA { @@ -64,4 +66,4 @@ class SAMDCurrentSenseADCDMA -#endif \ No newline at end of file +#endif From 063d1d5b4902452a0847c6b9ad8855c20b2aecce Mon Sep 17 00:00:00 2001 From: Chimera <50834634+Polyphe@users.noreply.github.com> Date: Sat, 30 Oct 2021 15:19:14 +0200 Subject: [PATCH 002/474] Update generic_mcu.cpp Add NRF52 series --- src/drivers/hardware_specific/generic_mcu.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 4a2360d9..fe1c51f4 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -20,6 +20,8 @@ #elif defined(TARGET_RP2040) +#elif defined(NRF52_SERIES) + #else // function setting the high pwm frequency to the supplied pins From 8e7750830cfb1f2bbd895c7025e640aaf6c6e1ec Mon Sep 17 00:00:00 2001 From: Chimera <50834634+Polyphe@users.noreply.github.com> Date: Sat, 30 Oct 2021 15:23:27 +0200 Subject: [PATCH 003/474] Add files via upload Add PWM for NRF52 series, that works on NRF52832, and Micro:bit V2 (NRF52833), should work on NRF52840 too. --- src/drivers/hardware_specific/nrf52_mcu.cpp | 353 ++++++++++++++++++++ 1 file changed, 353 insertions(+) create mode 100644 src/drivers/hardware_specific/nrf52_mcu.cpp diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp new file mode 100644 index 00000000..08cc2d5f --- /dev/null +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -0,0 +1,353 @@ +#include "../hardware_api.h" + +#if defined(NRF52_SERIES) + +#define PWM_CLK (16000000) +#define PWM_FREQ (40000) +#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ) +#define PWM_MAX_FREQ (62500) +#define DEAD_ZONE (250) // in ns +#define DEAD_TIME (DEAD_ZONE / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM + +#ifdef NRF_PWM3 +#define PWM_COUNT 4 +#else +#define PWM_COUNT 3 +#endif + +// empty motor slot +#define _EMPTY_SLOT (-0xAA) +#define _TAKEN_SLOT (-0x55) + +int pwm_range; +float dead_time; + +static NRF_PWM_Type* pwms[PWM_COUNT] = { + NRF_PWM0, + NRF_PWM1, + NRF_PWM2, + #ifdef NRF_PWM3 + NRF_PWM3 + #endif +}; + +typedef struct { + int pinA; + NRF_PWM_Type* mcpwm; + uint16_t mcpwm_channel_sequence[4]; +} bldc_3pwm_motor_slots_t; + +typedef struct { + int pin1A; + NRF_PWM_Type* mcpwm; + uint16_t mcpwm_channel_sequence[4]; +} stepper_motor_slots_t; + +typedef struct { + int pinAH; + NRF_PWM_Type* mcpwm1; + NRF_PWM_Type* mcpwm2; + uint16_t mcpwm_channel_sequence[8]; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0 + {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1 + {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2 + {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3 + }; + +// define stepper motor slots array +stepper_motor_slots_t nrf52_stepper_motor_slots[4] = { + {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0 + {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1 + {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2 + {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3 + }; + +// define BLDC motor slots array +bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1 + {_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2 + }; + +// configuring high frequency pwm timer +void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){ + + mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos); + mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos); + mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); + mcpwm1->COUNTERTOP = pwm_range; //pwm freq. + mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos); + mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos); + mcpwm1->SEQ[0].REFRESH = 0; + mcpwm1->SEQ[0].ENDDELAY = 0; + + if(mcpwm1 != mcpwm2){ + mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos); + mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos); + mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); + mcpwm2->COUNTERTOP = pwm_range; //pwm freq. + mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos); + mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos); + mcpwm2->SEQ[0].REFRESH = 0; + mcpwm2->SEQ[0].ENDDELAY = 0; + }else{ + mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos); + } +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 + else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + + int pA = g_ADigitalPinMap[pinA]; + int pB = g_ADigitalPinMap[pinB]; + int pC = g_ADigitalPinMap[pinC]; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + break; + } + } + // disable all the slots with the same MCPWM + if(slot_num < 2){ + // slot 0 of the stepper + nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~1UL; + }else{ + // slot 1 of the stepper + nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~2UL; + } + + // configure pwm outputs + + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC; + + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4; + + // configure the pwm + _configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm); +} + +// 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( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 + else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + + int pA = g_ADigitalPinMap[pinA]; + int pB = g_ADigitalPinMap[pinB]; + int pC = g_ADigitalPinMap[pinC]; + int pD = g_ADigitalPinMap[pinD]; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_stepper_motor_slots[slot_num].pin1A = pinA; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num < 2 ){ + // slots 0 and 1 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~1UL; + }else{ + // slots 2 and 3 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~2UL; + } + + // configure pwm outputs + + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD; + + nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4; + + // configure the pwm + _configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(nrf52_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,range] + + nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000; + nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000; + nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000; + + nrf52_bldc_3pwm_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1; + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(nrf52_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,range] + + nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000; + nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000; + nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000; + nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000; + + nrf52_stepper_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1; + break; + } + } +} + +/* Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +*/ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400 + else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + pwm_range /= 2; // scale the frequency (centered PWM) + + if (dead_zone != NOT_SET){ + dead_time = dead_zone/2; + }else{ + dead_time = DEAD_TIME/2; + } + + int pA_l = g_ADigitalPinMap[pinA_l]; + int pA_h = g_ADigitalPinMap[pinA_h]; + int pB_l = g_ADigitalPinMap[pinB_l]; + int pB_h = g_ADigitalPinMap[pinB_h]; + int pC_l = g_ADigitalPinMap[pinC_l]; + int pC_h = g_ADigitalPinMap[pinC_h]; + + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + break; + } + } + // if no slots available + if(slot_num >= 2) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 and 1 of the stepper + nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the stepper + nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT; + nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT; + } + + // Configure pwm outputs + + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4; + + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4]; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4; + + // Initializing the PPI peripheral for sync the pwm slots + + NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0]; + NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0]; + NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0]; + NRF_PPI->CHEN = 1UL << slot_num; + + // configure the pwm type + _configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2); + + // return + return 0; +} + +/* Function setting the duty cycle to the pwm pin +// - BLDC driver - 6PWM setting +// - hardware specific +*/ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, const int pinA_h, const int, const int, const int, const int, const int){ + for(int i = 0; i < 2; i++){ + if(nrf52_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,range] + + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000; + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range); + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000; + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range); + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000; + nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range); + + NRF_EGU0->TASKS_TRIGGER[0] = 1; + break; + } + } +} + + +#endif From 748ff92534f4821ba952aba303f49426242ae5b0 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 30 Oct 2021 22:33:57 -0100 Subject: [PATCH 004/474] fix #121 - add calls to Sensor::init() --- src/sensors/Encoder.cpp | 1 + src/sensors/HallSensor.cpp | 1 + src/sensors/MagneticSensorAnalog.cpp | 2 ++ src/sensors/MagneticSensorI2C.cpp | 2 ++ src/sensors/MagneticSensorPWM.cpp | 2 ++ src/sensors/MagneticSensorSPI.cpp | 2 ++ 6 files changed, 10 insertions(+) diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 7c164878..96057d82 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -205,6 +205,7 @@ void Encoder::init(){ // change it if the mode is quadrature if(quadrature == Quadrature::ON) cpr = 4*cpr; + // we don't call Sensor::init() here because init is handled in Encoder class. } // function enabling hardware interrupts of the for the callback provided diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 467ba9e7..3d2ba42e 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -166,6 +166,7 @@ void HallSensor::init(){ pulse_timestamp = _micros(); + // we don't call Sensor::init() here because init is handled in HallSensor class. } // function enabling hardware interrupts for the callback provided diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 6d881657..d4adad60 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -24,6 +24,8 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ raw_count = getRawCount(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 6c61b8ce..af93b8cc 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -64,6 +64,8 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // I2C communication begin wire->begin(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 5088fa9e..c043e7ce 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -27,6 +27,8 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + + this->Sensor::init(); // call base class init } // get current angle (rad) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b7a9dd27..4e4f7083 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -74,6 +74,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // do any architectures need to set the clock divider for SPI? Why was this in the code? //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); + + this->Sensor::init(); // call base class init } // Shaft angle calculation From 0fe8e7cee4c1a15c85fd49b27a74d549996ed4e1 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 08:23:16 +0100 Subject: [PATCH 005/474] added driver limit to the openloop examples #124 --- .../open_loop_position_example.ino | 11 + .../open_loop_velocity_example.ino | 14 +- .../osc_control_examples/osc_fullcontrol.pd | 384 ++++++++++++++++++ .../simplefoc_osc_esp32_fullcontrol.ino | 302 ++++++++++++++ .../osc_control_examples/ssid.h_rename_me | 4 + 5 files changed, 712 insertions(+), 3 deletions(-) create mode 100644 examples/osc_control_examples/osc_fullcontrol.pd create mode 100644 examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino create mode 100644 examples/osc_control_examples/ssid.h_rename_me diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index a4756352..653a7c11 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -18,18 +18,28 @@ float target_position = 0; // instantiate the commander Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_position, cmd); } +void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; driver.init(); // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp motor.voltage_limit = 3; // [V] + // limit/set the velocity of the transition in between + // target angles motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::angle_openloop; @@ -39,6 +49,7 @@ void setup() { // add target command T command.add('T', doTarget, "target angle"); + command.add('L', doLimit, "voltage limit"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index f7a82eb3..4a8752c2 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -18,20 +18,27 @@ float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_position, cmd); } +void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; driver.init(); // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp motor.voltage_limit = 3; // [V] - motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::velocity_openloop; @@ -40,7 +47,8 @@ void setup() { motor.init(); // add target command T - command.add('T', doTarget, "target velocity"); + command.add('T', doTarget, "target angle"); + command.add('L', doLimit, "voltage limit"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/osc_control_examples/osc_fullcontrol.pd b/examples/osc_control_examples/osc_fullcontrol.pd new file mode 100644 index 00000000..ddbf2f99 --- /dev/null +++ b/examples/osc_control_examples/osc_fullcontrol.pd @@ -0,0 +1,384 @@ +#N struct text float x float y text t; +#N canvas 1842 146 1519 1052 12; +#X obj 501 697 cnv 15 193 209 empty empty Tuning\ M1 20 12 0 14 #e0e0e0 +#404040 0; +#X obj 787 477 mrpeach/unpackOSC; +#X obj 132 586 print oscin; +#X obj 787 504 print oscout; +#X obj 723 449 spigot; +#X obj 774 452 tgl 15 1 empty empty Debug 17 7 0 10 #fcfcfc #000000 +#000000 1 1; +#X msg 591 503 disconnect; +#X obj 132 558 spigot; +#X obj 114 562 tgl 15 1 empty empty Debug -34 6 0 10 #fcfcfc #000000 +#000000 0 1; +#X obj 132 531 mrpeach/unpackOSC; +#X obj 673 477 mrpeach/udpsend; +#X obj 132 496 mrpeach/udpreceive 8000; +#X obj 673 422 mrpeach/packOSC; +#X obj 1043 150 hsl 249 25 -5000 5000 0 0 empty empty Set\ Point\ M1 +-2 -8 0 10 #fcfcfc #000000 #000000 0 1; +#X obj 1044 197 hsl 247 25 -15 15 0 0 empty empty Set\ Point\ M2 -2 +-8 0 10 #fcfcfc #000000 #000000 12300 1; +#X obj 120 153 bng 53 250 50 0 empty empty STOP 14 26 0 10 #fc1204 +#000000 #ffffff; +#X obj 200 102 * 0.10472; +#X obj 202 169 hsl 235 30 -520 520 0 0 empty empty Set\ point\ (Velocity) +-2 -8 0 10 #fcfcfc #000000 #000000 11500 1; +#X obj 673 449 spigot; +#X obj 653 452 tgl 15 1 empty empty Enable\ send -71 6 0 10 #fcfcfc +#000000 #000000 1 1; +#X msg 484 478 connect 192.168.1.43 8000; +#X obj 673 395 speedlim 100; +#X obj 231 573 mrpeach/routeOSC /M1 /M2; +#X obj 231 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L +/C; +#X obj 326 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc +#000000 #000000 0 1; +#X obj 326 812 % 6.28319; +#X obj 326 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc +#000000 #000000 0.137548 256 3; +#X obj 113 804 nbx 7 27 -1e+37 1e+37 0 0 empty empty Set\ point 0 -15 +0 18 #fcfcfc #000000 #000000 0 256 3; +#X obj 326 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 0 +0 18 #fcfcfc #000000 #000000 -348.637 256 3; +#X obj 326 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 0 +0 18 #fcfcfc #000000 #000000 0.0649328 256 3; +#X obj 538 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 0.2 256 3; +#X obj 537 776 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 20 256 3; +#X obj 538 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 0.0001 256 3; +#X obj 539 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 0 +10 #fcfcfc #000000 #000000 1000 256 3; +#X obj 539 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 +0 10 #fcfcfc #000000 #000000 0.01 256 3; +#X obj 605 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 608 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 609 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 +0 10 #fcfcfc #000000 #000000 8 256 3; +#X obj 122 278 hradio 53 0 1 3 motorselect empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 0; +#X scalar text 172 305 \; \;; +#X obj 122 372 select 0 1 2; +#X msg 122 399 prefix /M?; +#X msg 149 423 prefix /M1; +#X msg 178 445 prefix /M2; +#X obj 789 422 mrpeach/packOSC; +#X obj 789 395 speedlim 100; +#X msg 592 531 typetags \$1; +#X obj 571 535 tgl 15 1 empty empty OSC\ type\ tags -80 7 0 10 #ffffff +#000000 #000000 1 1; +#X text 63 286 Choose Motor, f 7; +#X text 137 339 All, f 4; +#X text 191 339 M1, f 4; +#X text 247 339 M2, f 4; +#X text 152 77 RPM; +#X obj 1008 148 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 +#000000 #ffffff; +#X obj 1009 195 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 +#000000 #ffffff; +#X obj 74 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc #000000 +#000000 1; +#X text 8 711 Control; +#X text 67 752 Voltage; +#X text 124 753 Velocity; +#X text 189 754 Position; +#X obj 312 101 /; +#X obj 312 129 * 6.28319; +#X text 424 75 cm, f 4; +#X text 393 51 Wheel diameter; +#X obj 394 100 * 0.0314159; +#X msg 348 636 set \$1; +#X msg 376 637 set \$1; +#X msg 407 636 set \$1; +#X msg 435 636 set \$1; +#X msg 466 636 set \$1; +#X msg 495 636 set \$1; +#X msg 524 637 set \$1; +#X msg 554 637 set \$1; +#X msg 583 637 set \$1; +#X obj 75 898 s osctargetedout; +#X obj 75 866 prepend /M1/C; +#X obj 773 304 r osctargetedout; +#X obj 593 912 prepend /M1/K; +#X obj 602 925 prepend /M1/N; +#X obj 609 936 prepend /M1/L; +#X obj 564 976 s osctargetedout; +#X obj 1271 697 cnv 15 193 209 empty empty Tuning\ M2 20 12 0 14 #e0e0e0 +#404040 0; +#X obj 1001 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L +/C; +#X obj 1096 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc +#000000 #000000 0 1; +#X obj 1096 812 % 6.28319; +#X obj 1096 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc +#000000 #000000 -0.13018 256 3; +#X obj 883 804 nbx 7 27 -1e+37 1e+37 0 0 setpointin2 empty Set\ point +0 -15 0 18 #fcfcfc #000000 #000000 0 256 3; +#X obj 1096 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 +0 0 18 #fcfcfc #000000 #000000 -346.273 256 3; +#X obj 1096 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 +0 0 18 #fcfcfc #000000 #000000 0.0657255 256 3; +#X obj 1308 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 0.2 256 3; +#X obj 1308 778 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1308 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 0.001 256 3; +#X obj 1309 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 +0 10 #fcfcfc #000000 #000000 1000 256 3; +#X obj 1309 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 +0 10 #fcfcfc #000000 #000000 0.01 256 3; +#X obj 1375 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 +-8 0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1378 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1379 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 +0 10 #fcfcfc #000000 #000000 8 256 3; +#X obj 844 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 1; +#X text 778 711 Control; +#X text 837 752 Voltage; +#X text 894 753 Velocity; +#X text 959 754 Position; +#X msg 1118 636 set \$1; +#X msg 1146 637 set \$1; +#X msg 1177 636 set \$1; +#X msg 1205 636 set \$1; +#X msg 1236 636 set \$1; +#X msg 1265 636 set \$1; +#X msg 1294 637 set \$1; +#X msg 1324 637 set \$1; +#X msg 1353 637 set \$1; +#X obj 845 898 s osctargetedout; +#X obj 1325 976 s osctargetedout; +#X obj 1379 936 prepend /M2/L; +#X obj 1372 925 prepend /M2/N; +#X obj 1364 912 prepend /M2/K; +#X obj 1296 947 prepend /M2/F; +#X obj 1291 940 prepend /M2/R; +#X obj 1287 933 prepend /M2/D; +#X obj 1281 925 prepend /M2/I; +#X obj 1276 917 prepend /M2/P; +#X obj 526 947 prepend /M1/F; +#X obj 521 940 prepend /M1/R; +#X obj 517 933 prepend /M1/D; +#X obj 511 925 prepend /M1/I; +#X obj 506 917 prepend /M1/P; +#X obj 393 78 nbx 2 14 0 50 0 1 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 6 256 2; +#X obj 299 71 nbx 5 24 -20 20 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 -0.266666 256 2; +#X obj 179 74 nbx 7 23 -5000 5000 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 -84.8824 256 2; +#X obj 577 169 hsl 104 30 -3.1415 3.1415 0 0 empty empty Set\ point\ (Position) +-2 -8 0 10 #fcfcfc #000000 #000000 10300 1; +#X obj 708 68 vsl 31 122 0 12 0 0 empty empty Set\ point\ (Voltage) +0 -9 0 10 #fcfcfc #000000 #000000 0 1; +#X obj 246 13 / 0.10472; +#X msg 318 13 set \$1; +#X obj 457 11 *; +#X obj 384 11 / 6.28319; +#X obj 547 96 /; +#X obj 547 125 * 6.28319; +#X obj 547 66 nbx 5 24 -100 100 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 0.0599999 256 2; +#X text 534 68 m; +#X obj 20 6 r setpointin; +#X obj 17 36 r setpointin2; +#X obj 120 6 spigot; +#X obj 120 42 spigot; +#X obj 16 63 r motorselect; +#X obj 1339 99 r setpointin; +#X obj 1345 150 r setpointin2; +#X msg 493 11 set \$1; +#X obj 22 103 > 1; +#X obj 59 103 <= 1; +#X msg 1110 529 /M?/params; +#X obj 1110 502 loadbang; +#X msg 1339 126 set \$1; +#X msg 1343 174 set \$1; +#X obj 639 9 *; +#X obj 566 9 / 6.28319; +#X msg 675 9 set \$1; +#X obj 483 448 loadbang; +#X text 284 74 m/s; +#X obj 845 866 prepend /M2/C; +#X msg 163 226 sendtyped /M?/t f 0; +#X obj 458 224 prepend sendtyped /t f; +#X msg 991 301 sendtyped /M2/t f 0; +#X msg 983 274 sendtyped /M1/t f 0; +#X obj 1051 286 prepend sendtyped /M1/t f; +#X obj 1047 322 prepend sendtyped /M2/t f; +#X obj 475 171 nbx 7 21 -1e+37 1e+37 0 0 empty empty rad 0 -8 0 10 +#fcfcfc #000000 #000000 2 256 2; +#X connect 1 0 3 0; +#X connect 4 0 1 0; +#X connect 5 0 4 1; +#X connect 6 0 10 0; +#X connect 7 0 2 0; +#X connect 8 0 7 1; +#X connect 9 0 7 0; +#X connect 9 0 22 0; +#X connect 11 0 9 0; +#X connect 12 0 18 0; +#X connect 12 0 4 0; +#X connect 13 0 163 0; +#X connect 14 0 164 0; +#X connect 15 0 159 0; +#X connect 16 0 17 0; +#X connect 17 0 131 0; +#X connect 17 0 134 0; +#X connect 17 0 160 0; +#X connect 18 0 10 0; +#X connect 19 0 18 1; +#X connect 20 0 10 0; +#X connect 21 0 12 0; +#X connect 22 0 23 0; +#X connect 22 1 82 0; +#X connect 23 0 26 0; +#X connect 23 1 29 0; +#X connect 23 2 28 0; +#X connect 23 3 27 0; +#X connect 23 4 65 0; +#X connect 23 5 66 0; +#X connect 23 6 67 0; +#X connect 23 7 68 0; +#X connect 23 8 69 0; +#X connect 23 9 70 0; +#X connect 23 10 71 0; +#X connect 23 11 72 0; +#X connect 23 12 73 0; +#X connect 25 0 24 0; +#X connect 28 0 25 0; +#X connect 30 0 125 0; +#X connect 31 0 124 0; +#X connect 32 0 123 0; +#X connect 33 0 122 0; +#X connect 34 0 121 0; +#X connect 35 0 77 0; +#X connect 36 0 78 0; +#X connect 37 0 79 0; +#X connect 38 0 40 0; +#X connect 40 0 41 0; +#X connect 40 1 42 0; +#X connect 40 2 43 0; +#X connect 41 0 12 0; +#X connect 42 0 12 0; +#X connect 43 0 12 0; +#X connect 44 0 18 0; +#X connect 44 0 4 0; +#X connect 45 0 44 0; +#X connect 46 0 12 0; +#X connect 47 0 46 0; +#X connect 53 0 162 0; +#X connect 54 0 161 0; +#X connect 55 0 75 0; +#X connect 60 0 61 0; +#X connect 61 0 17 0; +#X connect 64 0 60 1; +#X connect 64 0 133 1; +#X connect 64 0 135 1; +#X connect 64 0 153 1; +#X connect 65 0 30 0; +#X connect 66 0 31 0; +#X connect 67 0 32 0; +#X connect 68 0 33 0; +#X connect 69 0 34 0; +#X connect 70 0 35 0; +#X connect 71 0 36 0; +#X connect 72 0 37 0; +#X connect 73 0 55 0; +#X connect 75 0 74 0; +#X connect 76 0 45 0; +#X connect 77 0 80 0; +#X connect 78 0 80 0; +#X connect 79 0 80 0; +#X connect 82 0 85 0; +#X connect 82 1 88 0; +#X connect 82 2 87 0; +#X connect 82 3 86 0; +#X connect 82 4 102 0; +#X connect 82 5 103 0; +#X connect 82 6 104 0; +#X connect 82 7 105 0; +#X connect 82 8 106 0; +#X connect 82 9 107 0; +#X connect 82 10 108 0; +#X connect 82 11 109 0; +#X connect 82 12 110 0; +#X connect 84 0 83 0; +#X connect 87 0 84 0; +#X connect 89 0 120 0; +#X connect 90 0 119 0; +#X connect 91 0 118 0; +#X connect 92 0 117 0; +#X connect 93 0 116 0; +#X connect 94 0 115 0; +#X connect 95 0 114 0; +#X connect 96 0 113 0; +#X connect 97 0 158 0; +#X connect 102 0 89 0; +#X connect 103 0 90 0; +#X connect 104 0 91 0; +#X connect 105 0 92 0; +#X connect 106 0 93 0; +#X connect 107 0 94 0; +#X connect 108 0 95 0; +#X connect 109 0 96 0; +#X connect 110 0 97 0; +#X connect 113 0 112 0; +#X connect 114 0 112 0; +#X connect 115 0 112 0; +#X connect 116 0 112 0; +#X connect 117 0 112 0; +#X connect 118 0 112 0; +#X connect 119 0 112 0; +#X connect 120 0 112 0; +#X connect 121 0 80 0; +#X connect 122 0 80 0; +#X connect 123 0 80 0; +#X connect 124 0 80 0; +#X connect 125 0 80 0; +#X connect 126 0 64 0; +#X connect 127 0 60 0; +#X connect 128 0 16 0; +#X connect 129 0 165 0; +#X connect 130 0 160 0; +#X connect 131 0 132 0; +#X connect 132 0 128 0; +#X connect 133 0 146 0; +#X connect 134 0 133 0; +#X connect 135 0 136 0; +#X connect 136 0 165 0; +#X connect 137 0 135 0; +#X connect 139 0 141 0; +#X connect 140 0 142 0; +#X connect 143 0 147 0; +#X connect 143 0 148 0; +#X connect 144 0 151 0; +#X connect 145 0 152 0; +#X connect 146 0 127 0; +#X connect 147 0 142 1; +#X connect 148 0 141 1; +#X connect 149 0 44 0; +#X connect 150 0 149 0; +#X connect 151 0 13 0; +#X connect 152 0 14 0; +#X connect 153 0 155 0; +#X connect 154 0 153 0; +#X connect 155 0 137 0; +#X connect 156 0 20 0; +#X connect 158 0 111 0; +#X connect 159 0 44 0; +#X connect 160 0 21 0; +#X connect 161 0 44 0; +#X connect 162 0 44 0; +#X connect 163 0 45 0; +#X connect 164 0 45 0; +#X connect 165 0 160 0; +#X connect 165 0 154 0; diff --git a/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino b/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino new file mode 100644 index 00000000..a0a72336 --- /dev/null +++ b/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino @@ -0,0 +1,302 @@ +/** + * + * Control 2 motors on ESP32 using OSC + * + * In this example, all the commands available on the serial command interface are also available via OSC. + * Also, the example is for 2 motors. If you have only 1 motor, comment out the lines for the second motor. + * + * There is a simple GUI that works with 2 motors and all the commands, which you can find in the file + * "osc_fullcontrol.pd". You run it using purr Data: https://github.com/jonwwilkes/purr-data + * + * The nice thing about pD is that you can very easily extend the example using the graphical programming environment, + * and that is quick and easy to do. Everything is live, and you can change the program as it runs. There is a huge + * variety of existing functions and extensions, and its a very quick way to prototype some ideas. pD + OSC ss a good + * way to quickly connect to other devices / protocols. + * + * It is definately a solid choice for any kind of music/art project. + * + * + */ + + +#include "Arduino.h" +#include +#include +#include "ssid.h" // create this file, which defines the constants MYSSID and MYPASS +#include +#include + +#include +#include +#include + + +const char ssid[] = MYSSID; // your network SSID (name) +const char pass[] = MYPASS; // your network password +WiFiUDP Udp; +IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) +const unsigned int outPort = 8000; // remote port (not needed for receive) +const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) + + + + +MagneticSensorI2C sensor1 = MagneticSensorI2C(0x41, 14, 0xFE, 8); +MagneticSensorI2C sensor2 = MagneticSensorI2C(0x40, 14, 0xFE, 8); +BLDCMotor motor1 = BLDCMotor(11); +BLDCMotor motor2 = BLDCMotor(11); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(25, 26, 27); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(0, 4, 16); + +String m1Prefix("/M1"); +String m2Prefix("/M2"); + + +// we store seperate set-points for each motor, of course +float set_point1 = 2.0; +float set_point2 = 2.0; + + +OSCErrorCode error; +OSCBundle bundleOUT; // outgoing message, gets re-used + + + + + +void setup() { + // set pins low - motor initialization can take some time, + // and you don't want current flowing through the other motor while it happens... + pinMode(0,OUTPUT); + pinMode(4,OUTPUT); + pinMode(16,OUTPUT); + pinMode(25,OUTPUT); + pinMode(26,OUTPUT); + pinMode(27,OUTPUT); + digitalWrite(0, 0); + digitalWrite(4, 0); + digitalWrite(16, 0); + digitalWrite(25, 0); + digitalWrite(26, 0); + digitalWrite(27, 0); + + Serial.begin(115200); + delay(200); + + WiFi.begin(ssid, pass); + + Serial.print("Connecting WiFi "); + Serial.println(ssid); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); + } + Udp.begin(inPort); + Serial.println(); + Serial.print("WiFi connected. Local OSC address: "); + Serial.print(WiFi.localIP()); + Serial.print(":"); + Serial.println(inPort); + + + Serial.println("Initializing motors."); + + Wire.setClock(400000); + + sensor1.init(); + motor1.linkSensor(&sensor1); + driver1.voltage_power_supply = 9; + driver1.init(); + motor1.linkDriver(&driver1); + motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; + motor1.controller = ControlType::velocity; + motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.I = 20; + motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.output_ramp = 1000; + motor1.LPF_velocity.Tf = 0.01; + motor1.voltage_limit = 8; + motor1.P_angle.P = 20; + motor1.init(); + motor1.initFOC(); + + sensor2.init(); + motor2.linkSensor(&sensor2); + driver2.voltage_power_supply = 9; + driver2.init(); + motor2.linkDriver(&driver2); + motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; + motor2.controller = ControlType::velocity; + motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.I = 20; + motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.output_ramp = 1000; + motor2.LPF_velocity.Tf = 0.01; + motor2.voltage_limit = 8; + motor2.P_angle.P = 20; + motor2.init(); + motor2.initFOC(); + + sendMotorParams(motor1, m1Prefix); + sendMotorParams(motor2, m2Prefix); + Serial.println("All initialization complete."); + _delay(1000); +} + + + + +template +void sendMessage(String& addr, T datum) { + bundleOUT.add(addr.c_str()).add(datum); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + + + +void motorCmd(OSCMessage &msg, int offset, BLDCMotor& motor, float* set_point, String& prefix){ + Serial.print("Command for "); + Serial.println(prefix); + if (msg.fullMatch("/P", offset)) { + motor.PID_velocity.P = msg.getFloat(0); + sendMessage(prefix+"/P", motor.PID_velocity.P); + } + else if (msg.fullMatch("/I", offset)) { + motor.PID_velocity.I = msg.getFloat(0); + sendMessage(prefix+"/I", motor.PID_velocity.I); + } + else if (msg.fullMatch("/D", offset)) { + motor.PID_velocity.D = msg.getFloat(0); + sendMessage(prefix+"/D", motor.PID_velocity.D); + } + else if (msg.fullMatch("/R", offset)) { + motor.PID_velocity.output_ramp = msg.getFloat(0); + sendMessage(prefix+"/R", motor.PID_velocity.output_ramp); + } + else if (msg.fullMatch("/F", offset)) { + motor.LPF_velocity.Tf = msg.getFloat(0); + sendMessage(prefix+"/F", motor.LPF_velocity.Tf); + } + else if (msg.fullMatch("/K", offset)) { + motor.P_angle.P = msg.getFloat(0); + sendMessage(prefix+"/K", motor.P_angle.P); + } + else if (msg.fullMatch("/N", offset)) { + motor.velocity_limit = msg.getFloat(0); + sendMessage(prefix+"/N", motor.velocity_limit); + } + else if (msg.fullMatch("/L", offset)) { + motor.voltage_limit = msg.getFloat(0); + sendMessage(prefix+"/L", motor.voltage_limit); + } + else if (msg.fullMatch("/C", offset)) { + motor.controller = (ControlType)msg.getInt(0); + sendMessage(prefix+"/C", motor.controller); + } + else if (msg.fullMatch("/t", offset)) { + *set_point = msg.getFloat(0); + sendMessage(prefix+"/3", *set_point); // TODO is the set-point the same as motor.target? + Serial.print("Setting set-point to "); + Serial.println(*set_point); + } + else if (msg.fullMatch("/params", offset)) { + sendMotorParams(motor, prefix); + } +} + + + + + + +void sendMotorMonitoring() { + //Serial.println("Sending monitoring..."); + bundleOUT.add("/M1/0").add(motor1.voltage_q); + bundleOUT.add("/M1/1").add(motor1.shaft_velocity); + bundleOUT.add("/M1/2").add(motor1.shaft_angle); + bundleOUT.add("/M1/3").add(motor1.target); + bundleOUT.add("/M2/0").add(motor2.voltage_q); + bundleOUT.add("/M2/1").add(motor2.shaft_velocity); + bundleOUT.add("/M2/2").add(motor2.shaft_angle); + bundleOUT.add("/M2/3").add(motor2.target); + // TODO pack it into one message bundleOUT.add("/M2/i").add(motor2.voltage_q).add(motor2.shaft_velocity).add(motor2.shaft_angle).add(motor2.target); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + +void sendMotorParams(BLDCMotor& motor, String& prefix) { + bundleOUT.add((prefix+"/P").c_str()).add(motor.PID_velocity.P); + bundleOUT.add((prefix+"/I").c_str()).add(motor.PID_velocity.I); + bundleOUT.add((prefix+"/D").c_str()).add(motor.PID_velocity.D); + bundleOUT.add((prefix+"/R").c_str()).add(motor.PID_velocity.output_ramp); + bundleOUT.add((prefix+"/F").c_str()).add(motor.LPF_velocity.Tf); + bundleOUT.add((prefix+"/K").c_str()).add(motor.P_angle.P); + bundleOUT.add((prefix+"/N").c_str()).add(motor.velocity_limit); + bundleOUT.add((prefix+"/L").c_str()).add(motor.voltage_limit); + bundleOUT.add((prefix+"/C").c_str()).add(motor.controller); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + + + +long lastSend = 0; +OSCMessage bundleIN; +int size; + + +void loop() { + + // FOC algorithm function + motor1.loopFOC(); + motor1.move(set_point1); + motor2.loopFOC(); + motor2.move(set_point2); + + + int size = Udp.parsePacket(); + if (size > 0) { + while (size--) { + bundleIN.fill(Udp.read()); + } + if (!bundleIN.hasError()) { + bundleIN.route("/M1", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor1, &set_point1, m1Prefix); }, 0); + bundleIN.route("/M2", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor2, &set_point2, m2Prefix); }, 0); + IPAddress ip = Udp.remoteIP(); + if (!( ip==outIp )) { + Serial.print("New connection from "); + Serial.println(ip); + outIp = ip; + sendMotorParams(motor1, m1Prefix); + sendMotorParams(motor2, m2Prefix); + } + } + else { + error = bundleIN.getError(); + Serial.print("error: "); + Serial.println(error); + } + bundleIN.empty(); + } + else { // don't receive and send in the same loop... + long now = millis(); + if (now - lastSend > 100) { + sendMotorMonitoring(); + lastSend = now; + } + } + +} diff --git a/examples/osc_control_examples/ssid.h_rename_me b/examples/osc_control_examples/ssid.h_rename_me new file mode 100644 index 00000000..9d3b03c6 --- /dev/null +++ b/examples/osc_control_examples/ssid.h_rename_me @@ -0,0 +1,4 @@ + +#define MYSSID "yourssid" +#define MYPASS "yourpassword" + From a1f19c9cdf5015bab9809bc8bbd4b7a7f91adf2c Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 08:25:33 +0100 Subject: [PATCH 006/474] merge issue --- .../osc_control_examples/osc_fullcontrol.pd | 384 ------------------ .../simplefoc_osc_esp32_fullcontrol.ino | 302 -------------- .../osc_control_examples/ssid.h_rename_me | 4 - 3 files changed, 690 deletions(-) delete mode 100644 examples/osc_control_examples/osc_fullcontrol.pd delete mode 100644 examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino delete mode 100644 examples/osc_control_examples/ssid.h_rename_me diff --git a/examples/osc_control_examples/osc_fullcontrol.pd b/examples/osc_control_examples/osc_fullcontrol.pd deleted file mode 100644 index ddbf2f99..00000000 --- a/examples/osc_control_examples/osc_fullcontrol.pd +++ /dev/null @@ -1,384 +0,0 @@ -#N struct text float x float y text t; -#N canvas 1842 146 1519 1052 12; -#X obj 501 697 cnv 15 193 209 empty empty Tuning\ M1 20 12 0 14 #e0e0e0 -#404040 0; -#X obj 787 477 mrpeach/unpackOSC; -#X obj 132 586 print oscin; -#X obj 787 504 print oscout; -#X obj 723 449 spigot; -#X obj 774 452 tgl 15 1 empty empty Debug 17 7 0 10 #fcfcfc #000000 -#000000 1 1; -#X msg 591 503 disconnect; -#X obj 132 558 spigot; -#X obj 114 562 tgl 15 1 empty empty Debug -34 6 0 10 #fcfcfc #000000 -#000000 0 1; -#X obj 132 531 mrpeach/unpackOSC; -#X obj 673 477 mrpeach/udpsend; -#X obj 132 496 mrpeach/udpreceive 8000; -#X obj 673 422 mrpeach/packOSC; -#X obj 1043 150 hsl 249 25 -5000 5000 0 0 empty empty Set\ Point\ M1 --2 -8 0 10 #fcfcfc #000000 #000000 0 1; -#X obj 1044 197 hsl 247 25 -15 15 0 0 empty empty Set\ Point\ M2 -2 --8 0 10 #fcfcfc #000000 #000000 12300 1; -#X obj 120 153 bng 53 250 50 0 empty empty STOP 14 26 0 10 #fc1204 -#000000 #ffffff; -#X obj 200 102 * 0.10472; -#X obj 202 169 hsl 235 30 -520 520 0 0 empty empty Set\ point\ (Velocity) --2 -8 0 10 #fcfcfc #000000 #000000 11500 1; -#X obj 673 449 spigot; -#X obj 653 452 tgl 15 1 empty empty Enable\ send -71 6 0 10 #fcfcfc -#000000 #000000 1 1; -#X msg 484 478 connect 192.168.1.43 8000; -#X obj 673 395 speedlim 100; -#X obj 231 573 mrpeach/routeOSC /M1 /M2; -#X obj 231 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L -/C; -#X obj 326 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc -#000000 #000000 0 1; -#X obj 326 812 % 6.28319; -#X obj 326 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc -#000000 #000000 0.137548 256 3; -#X obj 113 804 nbx 7 27 -1e+37 1e+37 0 0 empty empty Set\ point 0 -15 -0 18 #fcfcfc #000000 #000000 0 256 3; -#X obj 326 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 0 -0 18 #fcfcfc #000000 #000000 -348.637 256 3; -#X obj 326 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 0 -0 18 #fcfcfc #000000 #000000 0.0649328 256 3; -#X obj 538 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 0 -10 #fcfcfc #000000 #000000 0.2 256 3; -#X obj 537 776 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 0 -10 #fcfcfc #000000 #000000 20 256 3; -#X obj 538 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 0 -10 #fcfcfc #000000 #000000 0.0001 256 3; -#X obj 539 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 0 -10 #fcfcfc #000000 #000000 1000 256 3; -#X obj 539 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 -0 10 #fcfcfc #000000 #000000 0.01 256 3; -#X obj 605 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 -8 -0 10 #fcfcfc #000000 #000000 20 256 3; -#X obj 608 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 -0 10 #fcfcfc #000000 #000000 20 256 3; -#X obj 609 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 -0 10 #fcfcfc #000000 #000000 8 256 3; -#X obj 122 278 hradio 53 0 1 3 motorselect empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 0; -#X scalar text 172 305 \; \;; -#X obj 122 372 select 0 1 2; -#X msg 122 399 prefix /M?; -#X msg 149 423 prefix /M1; -#X msg 178 445 prefix /M2; -#X obj 789 422 mrpeach/packOSC; -#X obj 789 395 speedlim 100; -#X msg 592 531 typetags \$1; -#X obj 571 535 tgl 15 1 empty empty OSC\ type\ tags -80 7 0 10 #ffffff -#000000 #000000 1 1; -#X text 63 286 Choose Motor, f 7; -#X text 137 339 All, f 4; -#X text 191 339 M1, f 4; -#X text 247 339 M2, f 4; -#X text 152 77 RPM; -#X obj 1008 148 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 -#000000 #ffffff; -#X obj 1009 195 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 -#000000 #ffffff; -#X obj 74 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc #000000 -#000000 1; -#X text 8 711 Control; -#X text 67 752 Voltage; -#X text 124 753 Velocity; -#X text 189 754 Position; -#X obj 312 101 /; -#X obj 312 129 * 6.28319; -#X text 424 75 cm, f 4; -#X text 393 51 Wheel diameter; -#X obj 394 100 * 0.0314159; -#X msg 348 636 set \$1; -#X msg 376 637 set \$1; -#X msg 407 636 set \$1; -#X msg 435 636 set \$1; -#X msg 466 636 set \$1; -#X msg 495 636 set \$1; -#X msg 524 637 set \$1; -#X msg 554 637 set \$1; -#X msg 583 637 set \$1; -#X obj 75 898 s osctargetedout; -#X obj 75 866 prepend /M1/C; -#X obj 773 304 r osctargetedout; -#X obj 593 912 prepend /M1/K; -#X obj 602 925 prepend /M1/N; -#X obj 609 936 prepend /M1/L; -#X obj 564 976 s osctargetedout; -#X obj 1271 697 cnv 15 193 209 empty empty Tuning\ M2 20 12 0 14 #e0e0e0 -#404040 0; -#X obj 1001 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L -/C; -#X obj 1096 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc -#000000 #000000 0 1; -#X obj 1096 812 % 6.28319; -#X obj 1096 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc -#000000 #000000 -0.13018 256 3; -#X obj 883 804 nbx 7 27 -1e+37 1e+37 0 0 setpointin2 empty Set\ point -0 -15 0 18 #fcfcfc #000000 #000000 0 256 3; -#X obj 1096 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 -0 0 18 #fcfcfc #000000 #000000 -346.273 256 3; -#X obj 1096 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 -0 0 18 #fcfcfc #000000 #000000 0.0657255 256 3; -#X obj 1308 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 -0 10 #fcfcfc #000000 #000000 0.2 256 3; -#X obj 1308 778 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 -0 10 #fcfcfc #000000 #000000 20 256 3; -#X obj 1308 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 -0 10 #fcfcfc #000000 #000000 0.001 256 3; -#X obj 1309 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 -0 10 #fcfcfc #000000 #000000 1000 256 3; -#X obj 1309 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 -0 10 #fcfcfc #000000 #000000 0.01 256 3; -#X obj 1375 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 --8 0 10 #fcfcfc #000000 #000000 20 256 3; -#X obj 1378 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 -0 10 #fcfcfc #000000 #000000 20 256 3; -#X obj 1379 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 -0 10 #fcfcfc #000000 #000000 8 256 3; -#X obj 844 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 1; -#X text 778 711 Control; -#X text 837 752 Voltage; -#X text 894 753 Velocity; -#X text 959 754 Position; -#X msg 1118 636 set \$1; -#X msg 1146 637 set \$1; -#X msg 1177 636 set \$1; -#X msg 1205 636 set \$1; -#X msg 1236 636 set \$1; -#X msg 1265 636 set \$1; -#X msg 1294 637 set \$1; -#X msg 1324 637 set \$1; -#X msg 1353 637 set \$1; -#X obj 845 898 s osctargetedout; -#X obj 1325 976 s osctargetedout; -#X obj 1379 936 prepend /M2/L; -#X obj 1372 925 prepend /M2/N; -#X obj 1364 912 prepend /M2/K; -#X obj 1296 947 prepend /M2/F; -#X obj 1291 940 prepend /M2/R; -#X obj 1287 933 prepend /M2/D; -#X obj 1281 925 prepend /M2/I; -#X obj 1276 917 prepend /M2/P; -#X obj 526 947 prepend /M1/F; -#X obj 521 940 prepend /M1/R; -#X obj 517 933 prepend /M1/D; -#X obj 511 925 prepend /M1/I; -#X obj 506 917 prepend /M1/P; -#X obj 393 78 nbx 2 14 0 50 0 1 empty empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 6 256 2; -#X obj 299 71 nbx 5 24 -20 20 0 0 empty empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 -0.266666 256 2; -#X obj 179 74 nbx 7 23 -5000 5000 0 0 empty empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 -84.8824 256 2; -#X obj 577 169 hsl 104 30 -3.1415 3.1415 0 0 empty empty Set\ point\ (Position) --2 -8 0 10 #fcfcfc #000000 #000000 10300 1; -#X obj 708 68 vsl 31 122 0 12 0 0 empty empty Set\ point\ (Voltage) -0 -9 0 10 #fcfcfc #000000 #000000 0 1; -#X obj 246 13 / 0.10472; -#X msg 318 13 set \$1; -#X obj 457 11 *; -#X obj 384 11 / 6.28319; -#X obj 547 96 /; -#X obj 547 125 * 6.28319; -#X obj 547 66 nbx 5 24 -100 100 0 0 empty empty empty 0 -8 0 10 #fcfcfc -#000000 #000000 0.0599999 256 2; -#X text 534 68 m; -#X obj 20 6 r setpointin; -#X obj 17 36 r setpointin2; -#X obj 120 6 spigot; -#X obj 120 42 spigot; -#X obj 16 63 r motorselect; -#X obj 1339 99 r setpointin; -#X obj 1345 150 r setpointin2; -#X msg 493 11 set \$1; -#X obj 22 103 > 1; -#X obj 59 103 <= 1; -#X msg 1110 529 /M?/params; -#X obj 1110 502 loadbang; -#X msg 1339 126 set \$1; -#X msg 1343 174 set \$1; -#X obj 639 9 *; -#X obj 566 9 / 6.28319; -#X msg 675 9 set \$1; -#X obj 483 448 loadbang; -#X text 284 74 m/s; -#X obj 845 866 prepend /M2/C; -#X msg 163 226 sendtyped /M?/t f 0; -#X obj 458 224 prepend sendtyped /t f; -#X msg 991 301 sendtyped /M2/t f 0; -#X msg 983 274 sendtyped /M1/t f 0; -#X obj 1051 286 prepend sendtyped /M1/t f; -#X obj 1047 322 prepend sendtyped /M2/t f; -#X obj 475 171 nbx 7 21 -1e+37 1e+37 0 0 empty empty rad 0 -8 0 10 -#fcfcfc #000000 #000000 2 256 2; -#X connect 1 0 3 0; -#X connect 4 0 1 0; -#X connect 5 0 4 1; -#X connect 6 0 10 0; -#X connect 7 0 2 0; -#X connect 8 0 7 1; -#X connect 9 0 7 0; -#X connect 9 0 22 0; -#X connect 11 0 9 0; -#X connect 12 0 18 0; -#X connect 12 0 4 0; -#X connect 13 0 163 0; -#X connect 14 0 164 0; -#X connect 15 0 159 0; -#X connect 16 0 17 0; -#X connect 17 0 131 0; -#X connect 17 0 134 0; -#X connect 17 0 160 0; -#X connect 18 0 10 0; -#X connect 19 0 18 1; -#X connect 20 0 10 0; -#X connect 21 0 12 0; -#X connect 22 0 23 0; -#X connect 22 1 82 0; -#X connect 23 0 26 0; -#X connect 23 1 29 0; -#X connect 23 2 28 0; -#X connect 23 3 27 0; -#X connect 23 4 65 0; -#X connect 23 5 66 0; -#X connect 23 6 67 0; -#X connect 23 7 68 0; -#X connect 23 8 69 0; -#X connect 23 9 70 0; -#X connect 23 10 71 0; -#X connect 23 11 72 0; -#X connect 23 12 73 0; -#X connect 25 0 24 0; -#X connect 28 0 25 0; -#X connect 30 0 125 0; -#X connect 31 0 124 0; -#X connect 32 0 123 0; -#X connect 33 0 122 0; -#X connect 34 0 121 0; -#X connect 35 0 77 0; -#X connect 36 0 78 0; -#X connect 37 0 79 0; -#X connect 38 0 40 0; -#X connect 40 0 41 0; -#X connect 40 1 42 0; -#X connect 40 2 43 0; -#X connect 41 0 12 0; -#X connect 42 0 12 0; -#X connect 43 0 12 0; -#X connect 44 0 18 0; -#X connect 44 0 4 0; -#X connect 45 0 44 0; -#X connect 46 0 12 0; -#X connect 47 0 46 0; -#X connect 53 0 162 0; -#X connect 54 0 161 0; -#X connect 55 0 75 0; -#X connect 60 0 61 0; -#X connect 61 0 17 0; -#X connect 64 0 60 1; -#X connect 64 0 133 1; -#X connect 64 0 135 1; -#X connect 64 0 153 1; -#X connect 65 0 30 0; -#X connect 66 0 31 0; -#X connect 67 0 32 0; -#X connect 68 0 33 0; -#X connect 69 0 34 0; -#X connect 70 0 35 0; -#X connect 71 0 36 0; -#X connect 72 0 37 0; -#X connect 73 0 55 0; -#X connect 75 0 74 0; -#X connect 76 0 45 0; -#X connect 77 0 80 0; -#X connect 78 0 80 0; -#X connect 79 0 80 0; -#X connect 82 0 85 0; -#X connect 82 1 88 0; -#X connect 82 2 87 0; -#X connect 82 3 86 0; -#X connect 82 4 102 0; -#X connect 82 5 103 0; -#X connect 82 6 104 0; -#X connect 82 7 105 0; -#X connect 82 8 106 0; -#X connect 82 9 107 0; -#X connect 82 10 108 0; -#X connect 82 11 109 0; -#X connect 82 12 110 0; -#X connect 84 0 83 0; -#X connect 87 0 84 0; -#X connect 89 0 120 0; -#X connect 90 0 119 0; -#X connect 91 0 118 0; -#X connect 92 0 117 0; -#X connect 93 0 116 0; -#X connect 94 0 115 0; -#X connect 95 0 114 0; -#X connect 96 0 113 0; -#X connect 97 0 158 0; -#X connect 102 0 89 0; -#X connect 103 0 90 0; -#X connect 104 0 91 0; -#X connect 105 0 92 0; -#X connect 106 0 93 0; -#X connect 107 0 94 0; -#X connect 108 0 95 0; -#X connect 109 0 96 0; -#X connect 110 0 97 0; -#X connect 113 0 112 0; -#X connect 114 0 112 0; -#X connect 115 0 112 0; -#X connect 116 0 112 0; -#X connect 117 0 112 0; -#X connect 118 0 112 0; -#X connect 119 0 112 0; -#X connect 120 0 112 0; -#X connect 121 0 80 0; -#X connect 122 0 80 0; -#X connect 123 0 80 0; -#X connect 124 0 80 0; -#X connect 125 0 80 0; -#X connect 126 0 64 0; -#X connect 127 0 60 0; -#X connect 128 0 16 0; -#X connect 129 0 165 0; -#X connect 130 0 160 0; -#X connect 131 0 132 0; -#X connect 132 0 128 0; -#X connect 133 0 146 0; -#X connect 134 0 133 0; -#X connect 135 0 136 0; -#X connect 136 0 165 0; -#X connect 137 0 135 0; -#X connect 139 0 141 0; -#X connect 140 0 142 0; -#X connect 143 0 147 0; -#X connect 143 0 148 0; -#X connect 144 0 151 0; -#X connect 145 0 152 0; -#X connect 146 0 127 0; -#X connect 147 0 142 1; -#X connect 148 0 141 1; -#X connect 149 0 44 0; -#X connect 150 0 149 0; -#X connect 151 0 13 0; -#X connect 152 0 14 0; -#X connect 153 0 155 0; -#X connect 154 0 153 0; -#X connect 155 0 137 0; -#X connect 156 0 20 0; -#X connect 158 0 111 0; -#X connect 159 0 44 0; -#X connect 160 0 21 0; -#X connect 161 0 44 0; -#X connect 162 0 44 0; -#X connect 163 0 45 0; -#X connect 164 0 45 0; -#X connect 165 0 160 0; -#X connect 165 0 154 0; diff --git a/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino b/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino deleted file mode 100644 index a0a72336..00000000 --- a/examples/osc_control_examples/simplefoc_osc_esp32_fullcontrol.ino +++ /dev/null @@ -1,302 +0,0 @@ -/** - * - * Control 2 motors on ESP32 using OSC - * - * In this example, all the commands available on the serial command interface are also available via OSC. - * Also, the example is for 2 motors. If you have only 1 motor, comment out the lines for the second motor. - * - * There is a simple GUI that works with 2 motors and all the commands, which you can find in the file - * "osc_fullcontrol.pd". You run it using purr Data: https://github.com/jonwwilkes/purr-data - * - * The nice thing about pD is that you can very easily extend the example using the graphical programming environment, - * and that is quick and easy to do. Everything is live, and you can change the program as it runs. There is a huge - * variety of existing functions and extensions, and its a very quick way to prototype some ideas. pD + OSC ss a good - * way to quickly connect to other devices / protocols. - * - * It is definately a solid choice for any kind of music/art project. - * - * - */ - - -#include "Arduino.h" -#include -#include -#include "ssid.h" // create this file, which defines the constants MYSSID and MYPASS -#include -#include - -#include -#include -#include - - -const char ssid[] = MYSSID; // your network SSID (name) -const char pass[] = MYPASS; // your network password -WiFiUDP Udp; -IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) -const unsigned int outPort = 8000; // remote port (not needed for receive) -const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) - - - - -MagneticSensorI2C sensor1 = MagneticSensorI2C(0x41, 14, 0xFE, 8); -MagneticSensorI2C sensor2 = MagneticSensorI2C(0x40, 14, 0xFE, 8); -BLDCMotor motor1 = BLDCMotor(11); -BLDCMotor motor2 = BLDCMotor(11); -BLDCDriver3PWM driver1 = BLDCDriver3PWM(25, 26, 27); -BLDCDriver3PWM driver2 = BLDCDriver3PWM(0, 4, 16); - -String m1Prefix("/M1"); -String m2Prefix("/M2"); - - -// we store seperate set-points for each motor, of course -float set_point1 = 2.0; -float set_point2 = 2.0; - - -OSCErrorCode error; -OSCBundle bundleOUT; // outgoing message, gets re-used - - - - - -void setup() { - // set pins low - motor initialization can take some time, - // and you don't want current flowing through the other motor while it happens... - pinMode(0,OUTPUT); - pinMode(4,OUTPUT); - pinMode(16,OUTPUT); - pinMode(25,OUTPUT); - pinMode(26,OUTPUT); - pinMode(27,OUTPUT); - digitalWrite(0, 0); - digitalWrite(4, 0); - digitalWrite(16, 0); - digitalWrite(25, 0); - digitalWrite(26, 0); - digitalWrite(27, 0); - - Serial.begin(115200); - delay(200); - - WiFi.begin(ssid, pass); - - Serial.print("Connecting WiFi "); - Serial.println(ssid); - while (WiFi.status() != WL_CONNECTED) { - delay(500); - Serial.print("."); - } - Udp.begin(inPort); - Serial.println(); - Serial.print("WiFi connected. Local OSC address: "); - Serial.print(WiFi.localIP()); - Serial.print(":"); - Serial.println(inPort); - - - Serial.println("Initializing motors."); - - Wire.setClock(400000); - - sensor1.init(); - motor1.linkSensor(&sensor1); - driver1.voltage_power_supply = 9; - driver1.init(); - motor1.linkDriver(&driver1); - motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor1.controller = ControlType::velocity; - motor1.PID_velocity.P = 0.2; - motor1.PID_velocity.I = 20; - motor1.PID_velocity.D = 0.001; - motor1.PID_velocity.output_ramp = 1000; - motor1.LPF_velocity.Tf = 0.01; - motor1.voltage_limit = 8; - motor1.P_angle.P = 20; - motor1.init(); - motor1.initFOC(); - - sensor2.init(); - motor2.linkSensor(&sensor2); - driver2.voltage_power_supply = 9; - driver2.init(); - motor2.linkDriver(&driver2); - motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor2.controller = ControlType::velocity; - motor2.PID_velocity.P = 0.2; - motor2.PID_velocity.I = 20; - motor2.PID_velocity.D = 0.001; - motor2.PID_velocity.output_ramp = 1000; - motor2.LPF_velocity.Tf = 0.01; - motor2.voltage_limit = 8; - motor2.P_angle.P = 20; - motor2.init(); - motor2.initFOC(); - - sendMotorParams(motor1, m1Prefix); - sendMotorParams(motor2, m2Prefix); - Serial.println("All initialization complete."); - _delay(1000); -} - - - - -template -void sendMessage(String& addr, T datum) { - bundleOUT.add(addr.c_str()).add(datum); - Udp.beginPacket(outIp, outPort); - bundleOUT.send(Udp); - Udp.endPacket(); - bundleOUT.empty(); // empty the bundle to free room for a new one -} - - - - - -void motorCmd(OSCMessage &msg, int offset, BLDCMotor& motor, float* set_point, String& prefix){ - Serial.print("Command for "); - Serial.println(prefix); - if (msg.fullMatch("/P", offset)) { - motor.PID_velocity.P = msg.getFloat(0); - sendMessage(prefix+"/P", motor.PID_velocity.P); - } - else if (msg.fullMatch("/I", offset)) { - motor.PID_velocity.I = msg.getFloat(0); - sendMessage(prefix+"/I", motor.PID_velocity.I); - } - else if (msg.fullMatch("/D", offset)) { - motor.PID_velocity.D = msg.getFloat(0); - sendMessage(prefix+"/D", motor.PID_velocity.D); - } - else if (msg.fullMatch("/R", offset)) { - motor.PID_velocity.output_ramp = msg.getFloat(0); - sendMessage(prefix+"/R", motor.PID_velocity.output_ramp); - } - else if (msg.fullMatch("/F", offset)) { - motor.LPF_velocity.Tf = msg.getFloat(0); - sendMessage(prefix+"/F", motor.LPF_velocity.Tf); - } - else if (msg.fullMatch("/K", offset)) { - motor.P_angle.P = msg.getFloat(0); - sendMessage(prefix+"/K", motor.P_angle.P); - } - else if (msg.fullMatch("/N", offset)) { - motor.velocity_limit = msg.getFloat(0); - sendMessage(prefix+"/N", motor.velocity_limit); - } - else if (msg.fullMatch("/L", offset)) { - motor.voltage_limit = msg.getFloat(0); - sendMessage(prefix+"/L", motor.voltage_limit); - } - else if (msg.fullMatch("/C", offset)) { - motor.controller = (ControlType)msg.getInt(0); - sendMessage(prefix+"/C", motor.controller); - } - else if (msg.fullMatch("/t", offset)) { - *set_point = msg.getFloat(0); - sendMessage(prefix+"/3", *set_point); // TODO is the set-point the same as motor.target? - Serial.print("Setting set-point to "); - Serial.println(*set_point); - } - else if (msg.fullMatch("/params", offset)) { - sendMotorParams(motor, prefix); - } -} - - - - - - -void sendMotorMonitoring() { - //Serial.println("Sending monitoring..."); - bundleOUT.add("/M1/0").add(motor1.voltage_q); - bundleOUT.add("/M1/1").add(motor1.shaft_velocity); - bundleOUT.add("/M1/2").add(motor1.shaft_angle); - bundleOUT.add("/M1/3").add(motor1.target); - bundleOUT.add("/M2/0").add(motor2.voltage_q); - bundleOUT.add("/M2/1").add(motor2.shaft_velocity); - bundleOUT.add("/M2/2").add(motor2.shaft_angle); - bundleOUT.add("/M2/3").add(motor2.target); - // TODO pack it into one message bundleOUT.add("/M2/i").add(motor2.voltage_q).add(motor2.shaft_velocity).add(motor2.shaft_angle).add(motor2.target); - Udp.beginPacket(outIp, outPort); - bundleOUT.send(Udp); - Udp.endPacket(); - bundleOUT.empty(); // empty the bundle to free room for a new one -} - - - -void sendMotorParams(BLDCMotor& motor, String& prefix) { - bundleOUT.add((prefix+"/P").c_str()).add(motor.PID_velocity.P); - bundleOUT.add((prefix+"/I").c_str()).add(motor.PID_velocity.I); - bundleOUT.add((prefix+"/D").c_str()).add(motor.PID_velocity.D); - bundleOUT.add((prefix+"/R").c_str()).add(motor.PID_velocity.output_ramp); - bundleOUT.add((prefix+"/F").c_str()).add(motor.LPF_velocity.Tf); - bundleOUT.add((prefix+"/K").c_str()).add(motor.P_angle.P); - bundleOUT.add((prefix+"/N").c_str()).add(motor.velocity_limit); - bundleOUT.add((prefix+"/L").c_str()).add(motor.voltage_limit); - bundleOUT.add((prefix+"/C").c_str()).add(motor.controller); - Udp.beginPacket(outIp, outPort); - bundleOUT.send(Udp); - Udp.endPacket(); - bundleOUT.empty(); // empty the bundle to free room for a new one -} - - - - - -long lastSend = 0; -OSCMessage bundleIN; -int size; - - -void loop() { - - // FOC algorithm function - motor1.loopFOC(); - motor1.move(set_point1); - motor2.loopFOC(); - motor2.move(set_point2); - - - int size = Udp.parsePacket(); - if (size > 0) { - while (size--) { - bundleIN.fill(Udp.read()); - } - if (!bundleIN.hasError()) { - bundleIN.route("/M1", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor1, &set_point1, m1Prefix); }, 0); - bundleIN.route("/M2", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor2, &set_point2, m2Prefix); }, 0); - IPAddress ip = Udp.remoteIP(); - if (!( ip==outIp )) { - Serial.print("New connection from "); - Serial.println(ip); - outIp = ip; - sendMotorParams(motor1, m1Prefix); - sendMotorParams(motor2, m2Prefix); - } - } - else { - error = bundleIN.getError(); - Serial.print("error: "); - Serial.println(error); - } - bundleIN.empty(); - } - else { // don't receive and send in the same loop... - long now = millis(); - if (now - lastSend > 100) { - sendMotorMonitoring(); - lastSend = now; - } - } - -} diff --git a/examples/osc_control_examples/ssid.h_rename_me b/examples/osc_control_examples/ssid.h_rename_me deleted file mode 100644 index 9d3b03c6..00000000 --- a/examples/osc_control_examples/ssid.h_rename_me +++ /dev/null @@ -1,4 +0,0 @@ - -#define MYSSID "yourssid" -#define MYPASS "yourpassword" - From 35ae1695716cb5e7bb99e8becd79eb8d796fc43f Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 08:39:30 +0100 Subject: [PATCH 007/474] bug in naming --- .../open_loop_position_example/open_loop_position_example.ino | 2 ++ .../open_loop_velocity_example/open_loop_velocity_example.ino | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 653a7c11..83b1fb32 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -19,6 +19,7 @@ float target_position = 0; Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_position, cmd); } void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } +void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); } void setup() { @@ -50,6 +51,7 @@ void setup() { // add target command T command.add('T', doTarget, "target angle"); command.add('L', doLimit, "voltage limit"); + command.add('V', doLimit, "movement velocity"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 4a8752c2..7a4a14ac 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -18,7 +18,7 @@ float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_position, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { @@ -47,7 +47,7 @@ void setup() { motor.init(); // add target command T - command.add('T', doTarget, "target angle"); + command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); Serial.begin(115200); From 4acf3d626011dd83b9c6567fdf57ad0c6f7cad07 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 17:22:51 +0100 Subject: [PATCH 008/474] voltage and current limiting handling bug #118 --- src/BLDCMotor.cpp | 5 +++-- src/common/defaults.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index ff451e07..d47ca51f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -42,11 +42,12 @@ void BLDCMotor::init() { // current control loop controls voltage PID_current_q.limit = voltage_limit; PID_current_d.limit = voltage_limit; + } + if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){ // velocity control loop controls current PID_velocity.limit = current_limit; - }else if(!current_sense && _isset(phase_resistance)){ - PID_velocity.limit = current_limit; }else{ + // velocity control loop controls the voltage PID_velocity.limit = voltage_limit; } P_angle.limit = velocity_limit; diff --git a/src/common/defaults.h b/src/common/defaults.h index df651e2a..c0a46182 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -28,7 +28,7 @@ #define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample From 92203b66869e5341424491001cbae56e6c92b820 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 17:26:26 +0100 Subject: [PATCH 009/474] initial support for common position+velocity+torque setting with commander #81 --- src/communication/Commander.cpp | 63 ++++++++++++++++++++++++++++++++- src/communication/Commander.h | 12 ++++++- 2 files changed, 73 insertions(+), 2 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 8e1cb051..e1d70d04 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -1,5 +1,5 @@ #include "Commander.h" - +#include Commander::Commander(Stream& serial, char eol, bool echo){ com_port = &serial; @@ -217,12 +217,18 @@ void Commander::motor(FOCMotor* motor, char* user_command) { switch(motor->torque_controller){ case TorqueControlType::voltage: println(F("volt")); + // change the velocity control limits if necessary + if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit; break; case TorqueControlType::dc_current: println(F("dc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; break; case TorqueControlType::foc_current: println(F("foc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; break; } break; @@ -439,6 +445,61 @@ void Commander::scalar(float* value, char* user_cmd){ println(*value); } + +void Commander::target(FOCMotor* motor, char* user_cmd){ + bool GET = isSentinel(user_cmd[0]); + if(!GET){ + float pos, vel, torque; + switch(motor->controller){ + case MotionControlType::torque: // setting torque target + torque= atof(strtok (user_cmd," ")); + motor->target = torque; + break; + case MotionControlType::velocity: // setting velocity target + torque limit + vel= atof(strtok (user_cmd," ")); + torque= atof(strtok (NULL," ")); + motor->target = vel; + motor->PID_velocity.limit = torque; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; + else motor->current_limit = torque; + break; + case MotionControlType::angle: // setting angle target + torque, velocity limit + pos= atof(strtok (user_cmd," ")); + vel= atof(strtok (NULL," ")); + torque= atof(strtok (NULL," ")); + motor->target = pos; + motor->velocity_limit = vel; + motor->P_angle.limit = vel; + motor->PID_velocity.limit = torque; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; + else motor->current_limit = torque; + break; + case MotionControlType::velocity_openloop: // setting velocity target + torque limit + vel= atof(strtok (user_cmd," ")); + torque= atof(strtok (NULL," ")); + motor->target = vel; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; + else motor->current_limit = torque; + break; + case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit + pos= atof(strtok (user_cmd," ")); + vel= atof(strtok (NULL," ")); + torque= atof(strtok (NULL," ")); + motor->target = pos; + motor->velocity_limit = vel; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; + else motor->current_limit = torque; + break; + } + } + //println(*value); +} + + bool Commander::isSentinel(char ch) { if(ch == eol) diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 13777867..018d7eb3 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -173,7 +173,17 @@ class Commander * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); - + /** + * Target setting interface, enables setting the target and limiting variables at once. + * The valeus are sent separated by a space. ex. P2.34 70 2 + * `P` is the user defined command, `2.34` is the target angle `70` is the target + * velocity and `2` is the desired max current. + * It depends of the motion control mode: + * - torque : torque (ex. P2.5) + * - velocity : velocity torque (ex.P10 2.5) + * - angle : angle velocity torque (ex.P3.5 10 2.5) + */ + void target(FOCMotor* motor, char* user_cmd); private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number From 8532cee9434f9eae1879709543b9e0fecd77bb02 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 17:33:54 +0100 Subject: [PATCH 010/474] removed string.h --- src/communication/Commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index e1d70d04..42dab7c3 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -1,5 +1,4 @@ #include "Commander.h" -#include Commander::Commander(Stream& serial, char eol, bool echo){ com_port = &serial; From a6075eca06e40a4811db97bda51b9110720928c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 31 Oct 2021 17:38:59 +0100 Subject: [PATCH 011/474] update release readme --- README.md | 30 +++++++----------------------- library.properties | 2 +- 2 files changed, 8 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index c0e019d6..f6557a4b 100644 --- a/README.md +++ b/README.md @@ -18,32 +18,16 @@ Therefore this is an attempt to:
-

NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

+

FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

    -
  • Sensor floating point error bugfix (initial solution) #83, #37
    • -
  • Sensor class restructuring - slight API change - docs
    • -
  • Restructured the generic code and simplified adding new mcus: IMPORTANT: an additional compiler flag needed for PlatformIO see issue and PlatformIO docs
    • -
  • Removed initial jump #110, #111
    • -
  • Double to float transformation of the code - performance increase by @sDessens (#100), @KaSroka (#100)
    • -
  • Docs webiste translated to Chinese! 🎉: Awesome work 😃 by @MINQING1101, @Deng-ge-open-source and @mingggggggg
    • -
  • New MCU support - docs -
      -
    • Support for arduino leonardo #108
      • -
    • Initial support for portenta h7 board in collaboration with Arduino
      • -
    • Initial support for esp8266
      • -
    -
    • -
  • Low side current sensing initial support - docs -
      -
    • Initial support for stm32 B_G431B_ESC1 by @sDessens: PR #73
      • -
    • Initial support for samd21 by @maxlem: PR #79
      • -
    • Initial support for esp32 by @byDagor
      • -
    -
    • +
  • Sensor class init bugfix #121
    • +
  • Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once
    • +
  • NRF52 series mcus support by @Polyphe
    • +
  • Voltage/current limit handling bugs #118
-## Arduino *SimpleFOClibrary* v2.1 +## Arduino *SimpleFOClibrary* v2.2

@@ -73,7 +57,7 @@ This video demonstrates the *Simple**FOC**library* basic usage, electronic conne

-## Arduino *SimpleFOCShield* v2.0.3 +## Arduino *SimpleFOCShield* v2.0.4

diff --git a/library.properties b/library.properties index 89540636..99ced852 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.2 +version=2.2.1 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 4555f919d8627138b6c728d308117cdf76101b6b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 1 Nov 2021 00:10:14 -0100 Subject: [PATCH 012/474] fix for #128 stm32g4xx current sense compile error --- src/current_sense/hardware_specific/stm32g4_hal.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/current_sense/hardware_specific/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32g4_hal.h index 13dd1de3..9368cb37 100644 --- a/src/current_sense/hardware_specific/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32g4_hal.h @@ -2,6 +2,10 @@ #define stm32g4_hal #if defined(STM32G4xx) + +#include +#include + void MX_GPIO_Init(void); void MX_DMA_Init(void); void MX_ADC1_Init(ADC_HandleTypeDef* hadc1); From 494ced58187b3a5116b515cefa10f1b6df61764f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Nov 2021 21:59:28 +0100 Subject: [PATCH 013/474] SAMD driver improvements --- src/drivers/hardware_specific/samd21_mcu.cpp | 11 +-- src/drivers/hardware_specific/samd51_mcu.cpp | 63 ++++++++++------- src/drivers/hardware_specific/samd_mcu.cpp | 71 +++++++++++++++----- src/drivers/hardware_specific/samd_mcu.h | 13 +++- 4 files changed, 108 insertions(+), 50 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index ab41b745..9be2eee0 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -159,6 +159,7 @@ void configureSAMDClock() { REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC | // Set the duty cycle to 50/50 HIGH/LOW GCLK_GENCTRL_GENEN | // Enable GCLK4 GCLK_GENCTRL_SRC_DFLL48M | // Set the 48MHz clock source + // GCLK_GENCTRL_SRC_FDPLL | // Set the 96MHz clock source GCLK_GENCTRL_ID(4); // Select GCLK4 while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization @@ -305,11 +306,11 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CC // tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); @@ -324,7 +325,7 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { // while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { - Tc* tc = (Tc*)GetTC(chaninfo); + Tc* tc = (Tc*)GetTC(info->tcc.chaninfo); tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index b59a43fc..19474d03 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -7,6 +7,16 @@ +// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency. +// for custom boards or overclockers you can override it using this define. +#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ +#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000 +#endif + + + + + // TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation // 0 6 8 24-bit Yes Yes Yes Yes Yes Yes // 1 4 8 24-bit Yes Yes Yes Yes Yes Yes @@ -17,7 +27,6 @@ #define NUM_WO_ASSOCIATIONS 72 - struct wo_association WO_associations[] = { { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, @@ -112,7 +121,7 @@ struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { EPioType getPeripheralOfPermutation(int permutation, int pin_position) { - return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; + return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT; } @@ -124,24 +133,17 @@ void syncTCC(Tcc* TCCx) { -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CCBUF - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! -// tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); -// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); -// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency! } - else { - // Tc* tc = (Tc*)GetTC(chaninfo); - // //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - // tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); - // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + else { + // we don't support the TC channels on SAMD51, isn't worth it. } } @@ -183,8 +185,8 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC - | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); - //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); + //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; syncTCC(tcc); // wait for sync + // work out pwm resolution for desired frequency and constrain to max/min values + long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolution0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } @@ -232,7 +243,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this? while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -254,11 +265,12 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { - Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); + //Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); // disable // tc->COUNT8.CTRLA.bit.ENABLE = 0; @@ -280,8 +292,9 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC "); SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51"); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index e1d38a80..5c0bb71e 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -315,9 +315,16 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -408,10 +415,18 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -479,11 +494,20 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); + getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res; + getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -579,6 +603,11 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC(s) configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1); if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo)) @@ -589,6 +618,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1); if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); + getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res; + getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it + getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res; + getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -611,9 +646,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const */ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); + writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tccI, dc_b); return; } @@ -635,11 +670,11 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { */ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); + writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tccI, dc_b); tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tccI, dc_c); return; } @@ -662,13 +697,13 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB */ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ tccConfiguration* tccI = getTccPinConfiguration(pin1A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); + writeSAMDDutyCycle(tccI, dc_1a); tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); + writeSAMDDutyCycle(tccI, dc_2a); tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); + writeSAMDDutyCycle(tccI, dc_1b); tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); + writeSAMDDutyCycle(tccI, dc_2b); return; } @@ -705,33 +740,33 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // low-side on a different pin of same TCC - do dead-time in software... float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_a); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly + writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly tcc1 = getTccPinConfiguration(pinB_h); tcc2 = getTccPinConfiguration(pinB_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_b); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tcc1, dc_b); tcc1 = getTccPinConfiguration(pinC_h); tcc2 = getTccPinConfiguration(pinC_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_c); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tcc1, dc_c); return; } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 7faf7442..8fafeaaa 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -29,9 +29,17 @@ #ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION #define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 -#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 #endif +#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000 +// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency +#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000 +// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency... +// 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency... +#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400 +// this is the most we can support on the TC units +#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 + #ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS #define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 #endif @@ -49,6 +57,7 @@ struct tccConfiguration { }; uint16_t chaninfo; } tcc; + uint16_t pwm_res; }; @@ -92,7 +101,7 @@ extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; struct wo_association& getWOAssociation(EPortType port, uint32_t pin); -void writeSAMDDutyCycle(int chaninfo, float dc); +void writeSAMDDutyCycle(tccConfiguration* info, float dc); void configureSAMDClock(); void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1); __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); From 64de2e4669e7ce18374d133291655068be0f0da9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Nov 2021 22:20:28 +0100 Subject: [PATCH 014/474] SAMD driver - pwm freq settable also on SAMD21 --- src/drivers/hardware_specific/samd21_mcu.cpp | 29 ++++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 9be2eee0..e4fb64b7 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -178,6 +178,15 @@ void configureSAMDClock() { * faster won't be possible without sacrificing resolution. */ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { + + long pwm_resolution = (24000000) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolution0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -262,7 +271,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } } @@ -280,8 +290,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, if (hw6pwm>0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } @@ -295,7 +305,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } @@ -316,8 +327,8 @@ void writeSAMDDutyCycle(tccConfiguration* info, float dc) { // uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); // while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + //while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); // tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); From 930a68607b6bd67f818964c66a4607647a3123ab Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 18 Nov 2021 00:13:58 +0100 Subject: [PATCH 015/474] fix nRF52 support to compile for Nano 33 (mbed) --- src/drivers/hardware_specific/nrf52_mcu.cpp | 28 +++++++++++---------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index 08cc2d5f..89e58e6c 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -1,7 +1,9 @@ + #include "../hardware_api.h" #if defined(NRF52_SERIES) + #define PWM_CLK (16000000) #define PWM_FREQ (40000) #define PWM_RESOLUTION (PWM_CLK/PWM_FREQ) @@ -108,9 +110,9 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pwm_range = (PWM_CLK / pwm_frequency); - int pA = g_ADigitalPinMap[pinA]; - int pB = g_ADigitalPinMap[pinB]; - int pC = g_ADigitalPinMap[pinC]; + int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA]; + int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB]; + int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC]; // determine which motor are we connecting // and set the appropriate configuration parameters @@ -159,10 +161,10 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pwm_range = (PWM_CLK / pwm_frequency); - int pA = g_ADigitalPinMap[pinA]; - int pB = g_ADigitalPinMap[pinB]; - int pC = g_ADigitalPinMap[pinC]; - int pD = g_ADigitalPinMap[pinD]; + int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA]; + int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB]; + int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC]; + int pD = digitalPinToPinName(pinD); //g_ADigitalPinMap[pinD]; // determine which motor are we connecting // and set the appropriate configuration parameters @@ -261,12 +263,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const dead_time = DEAD_TIME/2; } - int pA_l = g_ADigitalPinMap[pinA_l]; - int pA_h = g_ADigitalPinMap[pinA_h]; - int pB_l = g_ADigitalPinMap[pinB_l]; - int pB_h = g_ADigitalPinMap[pinB_h]; - int pC_l = g_ADigitalPinMap[pinC_l]; - int pC_h = g_ADigitalPinMap[pinC_h]; + int pA_l = digitalPinToPinName(pinA_l); //g_ADigitalPinMap[pinA_l]; + int pA_h = digitalPinToPinName(pinA_h); //g_ADigitalPinMap[pinA_h]; + int pB_l = digitalPinToPinName(pinB_l); //g_ADigitalPinMap[pinB_l]; + int pB_h = digitalPinToPinName(pinB_h); //g_ADigitalPinMap[pinB_h]; + int pC_l = digitalPinToPinName(pinC_l); //g_ADigitalPinMap[pinC_l]; + int pC_h = digitalPinToPinName(pinC_h); //g_ADigitalPinMap[pinC_h]; // determine which motor are we connecting From e80c72fb2b3b6803090a86cbd15931f0ec4199f7 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 27 Nov 2021 02:09:18 +0100 Subject: [PATCH 016/474] define sizes and explicit values for all enums --- src/common/base_classes/FOCMotor.h | 30 +++++++++++++++--------------- src/common/base_classes/Sensor.h | 12 ++++++------ src/communication/Commander.h | 8 ++++---- src/sensors/Encoder.h | 6 +++--- 4 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 37c5b260..8184ad1c 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -24,31 +24,31 @@ /** * Motiron control type */ -enum MotionControlType{ - torque,//!< Torque control - velocity,//!< Velocity motion control - angle,//!< Position/angle motion control - velocity_openloop, - angle_openloop +enum MotionControlType : uint8_t { + torque = 0x00, //!< Torque control + velocity = 0x01, //!< Velocity motion control + angle = 0x02, //!< Position/angle motion control + velocity_openloop = 0x03, + angle_openloop = 0x04 }; /** * Motiron control type */ -enum TorqueControlType{ - voltage, //!< Torque control using voltage - dc_current, //!< Torque control using DC current (one current magnitude) - foc_current //!< torque control using dq currents +enum TorqueControlType : uint8_t { + voltage = 0x00, //!< Torque control using voltage + dc_current = 0x01, //!< Torque control using DC current (one current magnitude) + foc_current = 0x02, //!< torque control using dq currents }; /** * FOC modulation type */ -enum FOCModulationType{ - SinePWM, //!< Sinusoidal PWM modulation - SpaceVectorPWM, //!< Space vector modulation method - Trapezoid_120, - Trapezoid_150 +enum FOCModulationType : uint8_t { + SinePWM = 0x00, //!< Sinusoidal PWM modulation + SpaceVectorPWM = 0x01, //!< Space vector modulation method + Trapezoid_120 = 0x02, + Trapezoid_150 = 0x03, }; /** diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index efab1be1..eb9608e8 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -6,19 +6,19 @@ /** * Direction structure */ -enum Direction{ - CW = 1, //clockwise +enum Direction : int8_t { + CW = 1, // clockwise CCW = -1, // counter clockwise - UNKNOWN = 0 //not yet known or invalid state + UNKNOWN = 0 // not yet known or invalid state }; /** * Pullup configuration structure */ -enum Pullup{ - USE_INTERN, //!< Use internal pullups - USE_EXTERN //!< Use external pullups +enum Pullup : uint8_t { + USE_INTERN = 0x00, //!< Use internal pullups + USE_EXTERN = 0x01 //!< Use external pullups }; /** diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 018d7eb3..924d7ae5 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -12,10 +12,10 @@ // Commander verbose display to the user type -enum VerboseMode{ - nothing = 0, // display nothing - good for monitoring - on_request, // display only on user request - user_friendly // display textual messages to the user +enum VerboseMode : uint8_t { + nothing = 0x00, // display nothing - good for monitoring + on_request = 0x01, // display only on user request + user_friendly = 0x02 // display textual messages to the user }; diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 3e775338..91427a83 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -10,9 +10,9 @@ /** * Quadrature mode configuration structure */ -enum Quadrature{ - ON, //!< Enable quadrature mode CPR = 4xPPR - OFF //!< Disable quadrature mode / CPR = PPR +enum Quadrature : uint8_t { + ON = 0x00, //!< Enable quadrature mode CPR = 4xPPR + OFF = 0x01 //!< Disable quadrature mode / CPR = PPR }; class Encoder: public Sensor{ From 009686acf1b2ebc9b70caf2693e9199321bac3db Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 6 Dec 2021 23:27:18 +0100 Subject: [PATCH 017/474] support motor intialization status tracking --- src/BLDCMotor.cpp | 11 +++++++++++ src/StepperMotor.cpp | 11 +++++++++++ src/common/base_classes/BLDCDriver.h | 2 ++ src/common/base_classes/FOCMotor.h | 16 ++++++++++++++++ src/common/base_classes/StepperDriver.h | 2 ++ src/drivers/BLDCDriver3PWM.cpp | 1 + src/drivers/BLDCDriver6PWM.cpp | 4 +++- src/drivers/StepperDriver2PWM.cpp | 1 + src/drivers/StepperDriver4PWM.cpp | 1 + 9 files changed, 48 insertions(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d47ca51f..6b46572d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -24,6 +24,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized")); + } + motor_status = FOCMotorStatus::motor_initializing; if(monitor_port) monitor_port->println(F("MOT: Init")); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit @@ -57,6 +62,7 @@ void BLDCMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); + motor_status = FOCMotorStatus::motor_uncalibrated; } @@ -87,6 +93,9 @@ void BLDCMotor::enable() // FOC initialization function int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + // align motor if necessary // alignment necessary for encoders! if(_isset(zero_electric_offset)){ @@ -117,9 +126,11 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); + motor_status = FOCMotorStatus::motor_ready; }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); + motor_status = FOCMotorStatus::motor_calib_failed; } return exit_flag; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2acd6f11..99667030 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -25,6 +25,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { // init hardware pins void StepperMotor::init() { + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized")); + } + motor_status = FOCMotorStatus::motor_initializing; if(monitor_port) monitor_port->println(F("MOT: Init")); // if set the phase resistance of the motor use current limit to calculate the voltage limit @@ -53,6 +58,7 @@ void StepperMotor::init() { enable(); _delay(500); + motor_status = FOCMotorStatus::motor_uncalibrated; } @@ -84,6 +90,9 @@ void StepperMotor::enable() // FOC initialization function int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + // align motor if necessary // alignment necessary for encoders! if(_isset(zero_electric_offset)){ @@ -105,8 +114,10 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); + motor_status = FOCMotorStatus::motor_ready; }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + motor_status = FOCMotorStatus::motor_calib_failed; disable(); } diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index a657d17c..3e220d2e 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -22,6 +22,8 @@ class BLDCDriver{ float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC + bool initialized = false; // true if driver was successfully initialized + /** * Set phase voltages to the harware * diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 8184ad1c..272b4074 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -51,6 +51,21 @@ enum FOCModulationType : uint8_t { Trapezoid_150 = 0x03, }; + + +enum FOCMotorStatus : uint8_t { + motor_uninitialized = 0x00, //!< Motor is not yet initialized + motor_initializing = 0x01, //!< Motor intiialization is in progress + motor_uncalibrated = 0x02, //!< Motor is initialized, but not calibrated (open loop possible) + motor_calibrating = 0x03, //!< Motor calibration in progress + motor_ready = 0x04, //!< Motor is initialized and calibrated (closed loop possible) + motor_error = 0x08, //!< Motor is in error state (recoverable, e.g. overcurrent protection active) + motor_calib_failed = 0x0E, //!< Motor calibration failed (possibly recoverable) + motor_init_failed = 0x0F, //!< Motor initialization failed (not recoverable) +}; + + + /** Generic motor class */ @@ -153,6 +168,7 @@ class FOCMotor // motor status vairables int8_t enabled = 0;//!< enabled or disabled motor flag + FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status // pwm modulation related variables FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 33f08843..aa9f9e4d 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -14,6 +14,8 @@ class StepperDriver{ 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 /** * Set phase voltages to the harware diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index bc239875..13cba6e8 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -57,6 +57,7 @@ int BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + initialized = true; // TODO atm the api gives no feedback if initialization succeeded return 0; } diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index eaeb2cfa..5b2ba9d9 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -58,7 +58,9 @@ int BLDCDriver6PWM::init() { // configure 6pwm // hardware specific function - depending on driver and mcu - return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + int result = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + initialized = (result==0); + return result; } // Set voltage to the pwm pin diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a0e61ca0..469ae694 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -80,6 +80,7 @@ int StepperDriver2PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu _configure2PWM(pwm_frequency, pwm1, pwm2); + initialized = true; // TODO atm the api gives no feedback if initialization succeeded return 0; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 6c905348..e4305e76 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -55,6 +55,7 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + initialized = true; // TODO atm the api gives no feedback if initialization succeeded return 0; } From eb682ab17618f30cfc401aa4313f88eb1b5dce00 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 7 Dec 2021 01:05:38 +0100 Subject: [PATCH 018/474] enable SPI sensor for RP2040, its now supported --- src/sensors/MagneticSensorSPI.cpp | 2 -- src/sensors/MagneticSensorSPI.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 4e4f7083..7341c89a 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -1,4 +1,3 @@ -#ifndef TARGET_RP2040 #include "MagneticSensorSPI.h" @@ -160,4 +159,3 @@ void MagneticSensorSPI::close(){ } -#endif diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 33aad3f3..a77e7698 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -1,7 +1,6 @@ #ifndef MAGNETICSENSORSPI_LIB_H #define MAGNETICSENSORSPI_LIB_H -#ifndef TARGET_RP2040 #include "Arduino.h" #include @@ -83,4 +82,3 @@ class MagneticSensorSPI: public Sensor{ #endif -#endif From 4fee8da4ef04501d1979e2b0e90457dfb44c118b Mon Sep 17 00:00:00 2001 From: SKURIC Antun Date: Fri, 17 Dec 2021 11:03:37 +0100 Subject: [PATCH 019/474] added implementations of generic position sensor and current sense --- .../generic_current_sense.ino | 59 +++++++ .../generic_sensor/generic_sensor.ino | 51 ++++++ keywords.txt | 4 + src/SimpleFOC.h | 2 + src/current_sense/GenericCurrentSense.cpp | 162 ++++++++++++++++++ src/current_sense/GenericCurrentSense.h | 41 +++++ src/sensors/GenericSensor.cpp | 26 +++ src/sensors/GenericSensor.h | 31 ++++ 8 files changed, 376 insertions(+) create mode 100644 examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino create mode 100644 examples/utils/sensor_test/generic_sensor/generic_sensor.ino create mode 100644 src/current_sense/GenericCurrentSense.cpp create mode 100644 src/current_sense/GenericCurrentSense.h create mode 100644 src/sensors/GenericSensor.cpp create mode 100644 src/sensors/GenericSensor.h diff --git a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino new file mode 100644 index 00000000..e999de23 --- /dev/null +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -0,0 +1,59 @@ +/** + * An example code for the generic current sensing implementation +*/ +#include + + +// user defined function for reading the phase currents +// returning the value per phase in amps +PhaseCurrent_s readCurrentSense(){ + PhaseCurrent_s c; + // dummy example only reading analog pins + c.a = analogRead(A0); + c.b = analogRead(A1); + c.c = analogRead(A2); // if no 3rd current sense set it to 0 + return(c); +} + +// user defined function for intialising the current sense +// it is optional and if provided it will be called in current_sense.init() +void initCurrentSense(){ + pinMode(A0,INPUT); + pinMode(A1,INPUT); + pinMode(A2,INPUT); +} + + +// GenericCurrentSense class constructor +// it receives the user defined callback for reading the current sense +// and optionally the user defined callback for current sense initialisation +GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense); + + +void setup() { + // if callbacks are not provided in the constructor + // they can be assigned directly: + //current_sense.readCallback = readCurrentSense; + //current_sense.initCallback = initCurrentSense; + + // initialise the current sensing + current_sense.init(); + + + Serial.begin(115200); + Serial.println("Current sense ready."); +} + +void loop() { + + PhaseCurrent_s currents = current_sense.getPhaseCurrents(); + float current_magnitude = current_sense.getDCCurrent(); + + Serial.print(currents.a); // milli Amps + Serial.print("\t"); + Serial.print(currents.b); // milli Amps + Serial.print("\t"); + Serial.print(currents.c); // milli Amps + Serial.print("\t"); + Serial.println(current_magnitude); // milli Amps +} \ No newline at end of file diff --git a/examples/utils/sensor_test/generic_sensor/generic_sensor.ino b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino new file mode 100644 index 00000000..4a470e59 --- /dev/null +++ b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino @@ -0,0 +1,51 @@ +/** + * Generic sensor example code + * + * This is a code intended to demonstrate how to implement the generic sensor class + * + */ + +#include + +// sensor reading function example +// for the magnetic sensor with analog communication +// returning an angle in radians in between 0 and 2PI +float readSensor(){ + return analogRead(A0)*_2PI/1024.0; +} + +// sensor intialising function +void initSensor(){ + pinMode(A0,INPUT); +} + +// generic sensor class contructor +// - read sensor callback +// - init sensor callback (optional) +GenericSensor sensor = GenericSensor(readSensor, initSensor); + +void setup() { + // monitoring port + Serial.begin(115200); + + // if callbacks are not provided in the constructor + // they can be assigned directly: + //sensor.readCallback = readSensor; + //sensor.initCallback = initSensor; + + sensor.init(); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} \ No newline at end of file diff --git a/keywords.txt b/keywords.txt index b47c18fd..a01ff880 100644 --- a/keywords.txt +++ b/keywords.txt @@ -22,6 +22,8 @@ LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 +GenericCurrentSense KEYWORD1 +GenericSensor KEYWORD1 initFOC KEYWORD2 @@ -102,6 +104,8 @@ lpf KEYWORD2 motor KEYWORD2 handlePWM KEYWORD2 enableInterrupt KEYWORD2 +readCallback KEYWORD2 +initCallback KEYWORD2 diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4f9f1108..3c24221c 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,12 +104,14 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" +#include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" #include "current_sense/LowsideCurrentSense.h" +#include "current_sense/GenericCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp new file mode 100644 index 00000000..a72d5232 --- /dev/null +++ b/src/current_sense/GenericCurrentSense.cpp @@ -0,0 +1,162 @@ +#include "GenericCurrentSense.h" + +// GenericCurrentSense constructor +GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){ + // if function provided add it to the + if(readCallback != nullptr) this->readCallback = readCallback; + if(initCallback != nullptr) this->initCallback = initCallback; +} + +// Inline sensor init function +void GenericCurrentSense::init(){ + // configure ADC variables + if(initCallback != nullptr) initCallback(); + // calibrate zero offsets + calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void GenericCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + PhaseCurrent_s current = readCallback(); + offset_ia += current.a; + offset_ib += current.b; + offset_ic += current.c; + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current = readCallback(); + current.a = (current.a - offset_ia);// amps + current.b = (current.a - offset_ib);// amps + current.c = (current.a - offset_ic); // amps + return current; +} + +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int GenericCurrentSense::driverSync(BLDCDriver *driver){ + return 1; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + int exit_flag = 1; + if(skip_align) return exit_flag; + + // // set phase A active and phases B and C down + // driver->setPwm(voltage, 0, 0); + // _delay(200); + // PhaseCurrent_s c = getPhaseCurrents(); + // // read the current 100 times ( arbitrary number ) + // for (int i = 0; i < 100; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.a = c.a*0.6f + 0.4f*c1.a; + // c.b = c.b*0.6f + 0.4f*c1.b; + // c.c = c.c*0.6f + 0.4f*c1.c; + // _delay(3); + // } + // driver->setPwm(0, 0, 0); + // // align phase A + // float ab_ratio = fabs(c.a / c.b); + // float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + // if( ab_ratio > 1.5f ){ // should be ~2 + // gain_a *= _sign(c.a); + // }else if( ab_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and B + // int tmp_pinA = pinA; + // pinA = pinB; + // pinB = tmp_pinA; + // gain_a *= _sign(c.b); + // exit_flag = 2; // signal that pins have been switched + // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and C + // int tmp_pinA = pinA; + // pinA = pinC; + // pinC= tmp_pinA; + // gain_a *= _sign(c.c); + // exit_flag = 2;// signal that pins have been switched + // }else{ + // // error in current sense - phase either not measured or bad connection + // return 0; + // } + + // // set phase B active and phases A and C down + // driver->setPwm(0, voltage, 0); + // _delay(200); + // c = getPhaseCurrents(); + // // read the current 50 times + // for (int i = 0; i < 100; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.a = c.a*0.6f + 0.4f*c1.a; + // c.b = c.b*0.6f + 0.4f*c1.b; + // c.c = c.c*0.6f + 0.4f*c1.c; + // _delay(3); + // } + // driver->setPwm(0, 0, 0); + // float ba_ratio = fabs(c.b/c.a); + // float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + // if( ba_ratio > 1.5f ){ // should be ~2 + // gain_b *= _sign(c.b); + // }else if( ba_ratio < 0.7f ){ // it should be ~0.5 + // // switch phase A and B + // int tmp_pinB = pinB; + // pinB = pinA; + // pinA = tmp_pinB; + // gain_b *= _sign(c.a); + // exit_flag = 2; // signal that pins have been switched + // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and C + // int tmp_pinB = pinB; + // pinB = pinC; + // pinC = tmp_pinB; + // gain_b *= _sign(c.c); + // exit_flag = 2; // signal that pins have been switched + // }else{ + // // error in current sense - phase either not measured or bad connection + // return 0; + // } + + // // if phase C measured + // if(_isset(pinC)){ + // // set phase B active and phases A and C down + // driver->setPwm(0, 0, voltage); + // _delay(200); + // c = getPhaseCurrents(); + // // read the adc voltage 500 times ( arbitrary number ) + // for (int i = 0; i < 50; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.c = (c.c+c1.c)/50.0f; + // } + // driver->setPwm(0, 0, 0); + // gain_c *= _sign(c.c); + // } + + // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + return exit_flag; +} diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h new file mode 100644 index 00000000..2f24e134 --- /dev/null +++ b/src/current_sense/GenericCurrentSense.h @@ -0,0 +1,41 @@ +#ifndef GENERIC_CS_LIB_H +#define GENERIC_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "../common/base_classes/CurrentSense.h" +#include "../common/lowpass_filter.h" +#include "hardware_api.h" + + +class GenericCurrentSense: public CurrentSense{ + public: + /** + GenericCurrentSense class constructor + */ + GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + + PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading + void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation + + private: + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/sensors/GenericSensor.cpp b/src/sensors/GenericSensor.cpp new file mode 100644 index 00000000..3d4025f3 --- /dev/null +++ b/src/sensors/GenericSensor.cpp @@ -0,0 +1,26 @@ +#include "GenericSensor.h" + + +/* + GenericSensor( float (*readCallback)() ) + - readCallback - pointer to the function which reads the sensor angle. +*/ + +GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){ + // if function provided add it to the + if(readCallback != nullptr) this->readCallback = readCallback; + if(initCallback != nullptr) this->initCallback = initCallback; +} + +void GenericSensor::init(){ + // if init callback specified run it + if(initCallback != nullptr) this->initCallback(); + this->Sensor::init(); // call base class init +} + +/* + Shaft angle calculation +*/ +float GenericSensor::getSensorAngle(){ + return this->readCallback(); +} \ No newline at end of file diff --git a/src/sensors/GenericSensor.h b/src/sensors/GenericSensor.h new file mode 100644 index 00000000..ba0dfe5c --- /dev/null +++ b/src/sensors/GenericSensor.h @@ -0,0 +1,31 @@ +#ifndef GENERIC_SENSOR_LIB_H +#define GENERIC_SENSOR_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" + + +class GenericSensor: public Sensor{ + public: + /** + GenericSensor class constructor + * @param readCallback pointer to the function reading the sensor angle + * @param initCallback pointer to the function initialising the sensor + */ + GenericSensor(float (*readCallback)() = nullptr, void (*initCallback)() = nullptr); + + float (*readCallback)() = nullptr; //!< function pointer to sensor reading + void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation + + void init() override; + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getSensorAngle() override; + +}; + + +#endif From 902c5a6844ce427d3007e26ac3d14b9d12aef539 Mon Sep 17 00:00:00 2001 From: SKURIC Antun Date: Fri, 17 Dec 2021 11:05:44 +0100 Subject: [PATCH 020/474] reade update generic sensors --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f6557a4b..a7329226 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,7 @@ Therefore this is an attempt to:

  • Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once
  • NRF52 series mcus support by @Polyphe
  • Voltage/current limit handling bugs #118
    • +
  • Generic position and current sense classes - to implement a new sensor only implement one function
    • From 7d04fa5ac0c5b935b4d5a9408079b49209740319 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 6 Jan 2022 21:34:43 +0100 Subject: [PATCH 021/474] separation of the esp32 mcpwm and ledc implementation --- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- .../hardware_specific/esp32_nomcpwm.cpp | 70 +++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 src/drivers/hardware_specific/esp32_nomcpwm.cpp diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f6663d87..90f14da4 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/esp32_nomcpwm.cpp b/src/drivers/hardware_specific/esp32_nomcpwm.cpp new file mode 100644 index 00000000..096092d5 --- /dev/null +++ b/src/drivers/hardware_specific/esp32_nomcpwm.cpp @@ -0,0 +1,70 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) !defined(SOC_MCPWM_SUPPORTED) + +#include "driver/ledc.h" + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution +#define _PWM_RES_BIT 10 // 10 bir resolution +#define _PWM_RES 1023 // 2^10-1 = 1023-1 + + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin, const int channel){ + ledcSetup(channel, freq, _PWM_RES_BIT ); + ledcAttachPin(pin, channel); +} + +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); + _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); +} + +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); + _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); + _setHighFrequency(pwm_frequency, pinC, LEDC_CHANNEL_2); +} + +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); + _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); + _setHighFrequency(pwm_frequency, pinC, LEDC_CHANNEL_2); + _setHighFrequency(pwm_frequency, pinD, LEDC_CHANNEL_3); +} + +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + uint32_t dutyA = _constrain(_PWM_RES*dc_a, 0, _PWM_RES); + uint32_t dutyB = _constrain(_PWM_RES*dc_b, 0, _PWM_RES); + ledcWrite(LEDC_CHANNEL_0, dutyA); + ledcWrite(LEDC_CHANNEL_1, dutyB); +} + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + uint32_t dutyA = _constrain(_PWM_RES*dc_a, 0, _PWM_RES); + uint32_t dutyB = _constrain(_PWM_RES*dc_b, 0, _PWM_RES); + uint32_t dutyC = _constrain(_PWM_RES*dc_c, 0, _PWM_RES); + ledcWrite(LEDC_CHANNEL_0, dutyA); + ledcWrite(LEDC_CHANNEL_1, dutyB); + ledcWrite(LEDC_CHANNEL_2, dutyC); +} + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + uint32_t duty1A = _constrain(_PWM_RES*dc_1a, 0, _PWM_RES); + uint32_t duty1B = _constrain(_PWM_RES*dc_1b, 0, _PWM_RES); + uint32_t duty2A = _constrain(_PWM_RES*dc_2a, 0, _PWM_RES); + uint32_t duty2B = _constrain(_PWM_RES*dc_2b, 0, _PWM_RES); + ledcWrite(LEDC_CHANNEL_0, duty1A); + ledcWrite(LEDC_CHANNEL_1, duty1B); + ledcWrite(LEDC_CHANNEL_2, duty2A); + ledcWrite(LEDC_CHANNEL_3, duty2B); +} + +#endif \ No newline at end of file From 140de3b456d06e0aa8f17626b3b83d9983d26afc Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 6 Jan 2022 21:35:18 +0100 Subject: [PATCH 022/474] separation of the esp32 mcpwm and ledc implementation - typo --- src/drivers/hardware_specific/esp32_nomcpwm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/esp32_nomcpwm.cpp b/src/drivers/hardware_specific/esp32_nomcpwm.cpp index 096092d5..e1313b58 100644 --- a/src/drivers/hardware_specific/esp32_nomcpwm.cpp +++ b/src/drivers/hardware_specific/esp32_nomcpwm.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) #include "driver/ledc.h" From a3d72c09e24395e9cc8e042d8c299ec5c94bc3a0 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 6 Jan 2022 21:38:53 +0100 Subject: [PATCH 023/474] inital navie support of the esp32s2 and esp32s3 - issue #120, #109 --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index a7329226..d1673651 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,7 @@ Therefore this is an attempt to:
  • NRF52 series mcus support by @Polyphe
  • Voltage/current limit handling bugs #118
  • Generic position and current sense classes - to implement a new sensor only implement one function
    • +
  • Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation
    • From 1ff4c06427e407266eff702684290d318f14aaf1 Mon Sep 17 00:00:00 2001 From: Sam Date: Fri, 7 Jan 2022 14:55:09 +0800 Subject: [PATCH 024/474] Modify PWM timer configuration according to API changes in esp32 official SDK 2.0.2 --- src/drivers/hardware_specific/esp32_mcu.cpp | 42 +++++++++++---------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f6663d87..12de06aa 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -124,7 +124,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.prescale = 0; + mcpwm_num->clk_cfg.clk_prescale = 0; // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 @@ -140,18 +140,18 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler - mcpwm_num->timer[0].period.prescale = prescaler; - mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; + mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; + mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; _delay(1); //set period - mcpwm_num->timer[0].period.period = resolution_corrected; - mcpwm_num->timer[1].period.period = resolution_corrected; - mcpwm_num->timer[2].period.period = resolution_corrected; + mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; + mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; + mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; _delay(1); - mcpwm_num->timer[0].period.upmethod = 0; - mcpwm_num->timer[1].period.upmethod = 0; - mcpwm_num->timer[2].period.upmethod = 0; + mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; + mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; + mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; _delay(1); // _delay(1); //restart the timers @@ -159,16 +159,18 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); _delay(1); - // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined - // in the default Espressif MCPWM headers. The correct const may be used - // when https://github.com/espressif/esp-idf/issues/5429 is resolved. - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, (mcpwm_sync_signal_t)1, 0); - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 1; - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 0; + + mcpwm_sync_config_t sync_conf = { + .sync_sig = MCPWM_SELECT_TIMER0_SYNC, + .timer_val = 0, + .count_direction = MCPWM_TIMER_DIRECTION_UP, + }; + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); + + // TIMER0 has itself as sync source, route this sync source directly to sync output for the other two + mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_SYNCIN); } // function setting the high pwm frequency to the supplied pins From bc87456d22072a4110fd96bfdd1b413e29224976 Mon Sep 17 00:00:00 2001 From: Sam Date: Fri, 7 Jan 2022 15:04:05 +0800 Subject: [PATCH 025/474] Refine code format --- src/drivers/hardware_specific/esp32_mcu.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 12de06aa..a58c3a1d 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -161,10 +161,10 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm _delay(1); mcpwm_sync_config_t sync_conf = { - .sync_sig = MCPWM_SELECT_TIMER0_SYNC, - .timer_val = 0, - .count_direction = MCPWM_TIMER_DIRECTION_UP, - }; + .sync_sig = MCPWM_SELECT_TIMER0_SYNC, + .timer_val = 0, + .count_direction = MCPWM_TIMER_DIRECTION_UP + }; mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); From dbececdfdd46659963c6899b8bf7a1d30b34bc2c Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jan 2022 08:08:25 +0100 Subject: [PATCH 026/474] esp32s2/s3 support for multiple motors + cs separation --- .../hardware_specific/esp32_adc_driver.cpp | 2 +- .../hardware_specific/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32_ledc_mcu.cpp | 30 +++ .../hardware_specific/esp32_mcu.cpp | 7 +- .../hardware_specific/esp32_ledc_mcu.cpp | 204 ++++++++++++++++++ src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- .../hardware_specific/esp32_nomcpwm.cpp | 70 ------ 7 files changed, 240 insertions(+), 77 deletions(-) create mode 100644 src/current_sense/hardware_specific/esp32_ledc_mcu.cpp create mode 100644 src/drivers/hardware_specific/esp32_ledc_mcu.cpp delete mode 100644 src/drivers/hardware_specific/esp32_nomcpwm.cpp diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 76ee54e9..40dc0140 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h index df8166f0..869c4dde 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp new file mode 100644 index 00000000..79d335e9 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp @@ -0,0 +1,30 @@ +#include "../hardware_api.h" +#include "../../drivers/hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + // uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} + +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB, const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} +#endif diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 7b9ba448..0996ef11 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,7 +1,9 @@ #include "../hardware_api.h" #include "../../drivers/hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) + +#include "esp32_adc_driver.h" #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" @@ -10,8 +12,6 @@ #include #include -#include "esp32_adc_driver.h" - #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f @@ -121,5 +121,4 @@ static void IRAM_ATTR isr_handler(void*){ if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } - #endif diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp new file mode 100644 index 00000000..7b10f464 --- /dev/null +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -0,0 +1,204 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)// && !defined(SOC_MCPWM_SUPPORTED) + +#include "driver/ledc.h" + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution +#define _PWM_RES_BIT 10 // 10 bir resolution +#define _PWM_RES 1023 // 2^10-1 = 1023-1 + + +// empty motor slot +#define _EMPTY_SLOT -20 +#define _TAKEN_SLOT -21 + +// figure out how many ledc channels are avaible +// esp32 - 2x8=16 +// esp32s2 - 8 +// esp32c3 - 6 +#include "soc/soc_caps.h" +#ifdef SOC_LEDC_SUPPORT_HS_MODE +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1) +#else +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) +#endif + +// current channel stack index +// support for multiple motors +// esp32 has 16 channels +// esp32s2 has 8 channels +// esp32c3 has 6 channels +int channel_index = 0; + +// slot for the 3pwm bldc motors +typedef struct { + int pinID; + int ch1; + int ch2; + int ch3; +} bldc_3pwm_motor_slots_t; + +// slot for the 2pwm stepper motors +typedef struct { + int pinID; + int ch1; + int ch2; +} stepper_2pwm_motor_slots_t; + +// slot for the 4pwm stepper motors +typedef struct { + int pinID; + int ch1; + int ch2; + int ch3; + int ch4; +} stepper_4pwm_motor_slots_t; + + +// define motor slots array +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, 0,0,0}, // 1st motor + {_EMPTY_SLOT, 0,0,0}, // 2nd motor + {_EMPTY_SLOT, 0,0,0}, // 3st motor // esp32s2 & esp32 + {_EMPTY_SLOT, 0,0,0}, // 4nd motor // esp32 only +}; +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4]={ + {_EMPTY_SLOT, 0,0}, // 1st motor + {_EMPTY_SLOT, 0,0}, // 2nd motor + {_EMPTY_SLOT, 0,0}, // 3rd motor + {_EMPTY_SLOT, 0,0} // 4th motor - esp32s2 and esp32 +}; +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[4]={ + {_EMPTY_SLOT, 0,0,0,0}, // 1st motor + {_EMPTY_SLOT, 0,0,0,0}, // 2nd motor - esp32s2 and esp32 + {_EMPTY_SLOT, 0,0,0,0}, // 3st motor - only esp32 + {_EMPTY_SLOT, 0,0,0,0}, // 4st motor - only esp32 +}; + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin, const int channel){ + ledcSetup(channel, freq, _PWM_RES_BIT ); + ledcAttachPin(pin, channel); +} + +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 2 >= LEDC_CHANNELS ) return; + + stepper_2pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_2pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_2pwm_motor_slots[slot_num].pinID = pinA; + esp32_stepper_2pwm_motor_slots[slot_num].ch1 = channel_index++; + esp32_stepper_2pwm_motor_slots[slot_num].ch2 = channel_index++; + m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; + break; + } + } + + _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); + _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); +} + +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 3 >= LEDC_CHANNELS ) return; + + bldc_3pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinID = pinA; + esp32_bldc_3pwm_motor_slots[slot_num].ch1 = channel_index++; + esp32_bldc_3pwm_motor_slots[slot_num].ch2 = channel_index++; + esp32_bldc_3pwm_motor_slots[slot_num].ch3 = channel_index++; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; + break; + } + } + + _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); + _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); + _setHighFrequency(pwm_frequency, pinC, m_slot.ch3); +} + +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 4 >= LEDC_CHANNELS ) return; + + + stepper_4pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_4pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_4pwm_motor_slots[slot_num].pinID = pinA; + esp32_stepper_4pwm_motor_slots[slot_num].ch1 = channel_index++; + esp32_stepper_4pwm_motor_slots[slot_num].ch2 = channel_index++; + esp32_stepper_4pwm_motor_slots[slot_num].ch3 = channel_index++; + esp32_stepper_4pwm_motor_slots[slot_num].ch4 = channel_index++; + m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; + break; + } + } + + _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); + _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); + _setHighFrequency(pwm_frequency, pinC, m_slot.ch3); + _setHighFrequency(pwm_frequency, pinD, m_slot.ch4); +} + +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +// determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_2pwm_motor_slots[i].pinID == pinA){ // if motor slot found + ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + break; + } + } +} + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_3pwm_motor_slots[i].pinID == pinA){ // if motor slot found + ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + break; + } + } +} + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_4pwm_motor_slots[i].pinID == pin1A){ // if motor slot found + ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); + ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); + ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); + ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch4, _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + break; + } + } +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 90f14da4..ace0fdfd 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && 0 #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/esp32_nomcpwm.cpp b/src/drivers/hardware_specific/esp32_nomcpwm.cpp deleted file mode 100644 index e1313b58..00000000 --- a/src/drivers/hardware_specific/esp32_nomcpwm.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "../hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) - -#include "driver/ledc.h" - -#define _PWM_FREQUENCY 25000 // 25khz -#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution -#define _PWM_RES_BIT 10 // 10 bir resolution -#define _PWM_RES 1023 // 2^10-1 = 1023-1 - - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); -} - -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); - _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); -} - -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); - _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); - _setHighFrequency(pwm_frequency, pinC, LEDC_CHANNEL_2); -} - -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA, LEDC_CHANNEL_0); - _setHighFrequency(pwm_frequency, pinB, LEDC_CHANNEL_1); - _setHighFrequency(pwm_frequency, pinC, LEDC_CHANNEL_2); - _setHighFrequency(pwm_frequency, pinD, LEDC_CHANNEL_3); -} - -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - uint32_t dutyA = _constrain(_PWM_RES*dc_a, 0, _PWM_RES); - uint32_t dutyB = _constrain(_PWM_RES*dc_b, 0, _PWM_RES); - ledcWrite(LEDC_CHANNEL_0, dutyA); - ledcWrite(LEDC_CHANNEL_1, dutyB); -} - -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - uint32_t dutyA = _constrain(_PWM_RES*dc_a, 0, _PWM_RES); - uint32_t dutyB = _constrain(_PWM_RES*dc_b, 0, _PWM_RES); - uint32_t dutyC = _constrain(_PWM_RES*dc_c, 0, _PWM_RES); - ledcWrite(LEDC_CHANNEL_0, dutyA); - ledcWrite(LEDC_CHANNEL_1, dutyB); - ledcWrite(LEDC_CHANNEL_2, dutyC); -} - -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - uint32_t duty1A = _constrain(_PWM_RES*dc_1a, 0, _PWM_RES); - uint32_t duty1B = _constrain(_PWM_RES*dc_1b, 0, _PWM_RES); - uint32_t duty2A = _constrain(_PWM_RES*dc_2a, 0, _PWM_RES); - uint32_t duty2B = _constrain(_PWM_RES*dc_2b, 0, _PWM_RES); - ledcWrite(LEDC_CHANNEL_0, duty1A); - ledcWrite(LEDC_CHANNEL_1, duty1B); - ledcWrite(LEDC_CHANNEL_2, duty2A); - ledcWrite(LEDC_CHANNEL_3, duty2B); -} - -#endif \ No newline at end of file From cb4da9c826bd28ea8626e7262809daa9f6a3e716 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jan 2022 08:13:12 +0100 Subject: [PATCH 027/474] forgotten define --- src/drivers/hardware_specific/esp32_ledc_mcu.cpp | 2 +- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index 7b10f464..56914d45 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)// && !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) #include "driver/ledc.h" diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index ace0fdfd..64fad94f 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" From 3816c823bfd4e1c0cdf6d1ba7597268519d145d9 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jan 2022 18:58:33 +0100 Subject: [PATCH 028/474] workflow update for esp32 package v2.0.2 and esp32s2 --- .github/workflows/ccpp.yml | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6171ab77..579939c4 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -13,6 +13,7 @@ jobs: - arduino:samd:nano_33_iot # samd21 - adafruit:samd:adafruit_metro_m4 # samd51 - esp32:esp32:esp32doit-devkit-v1 # esm32 + - esp32:esp32:esp32s2 # esm32s2 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - arduino:mbed_rp2040:pico # rpi pico @@ -36,15 +37,19 @@ jobs: platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json sketch-names: single_full_control_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # one full example + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino From e87dd58fb0441ad15e5b20c81c15e06a763ace2c Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jan 2022 19:36:08 +0100 Subject: [PATCH 029/474] update workflow + refactoring --- .github/workflows/ccpp.yml | 6 +++--- src/drivers/hardware_specific/esp32_ledc_mcu.cpp | 2 +- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 579939c4..306b4a39 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -12,8 +12,8 @@ jobs: - arduino:sam:arduino_due_x # arduino due - arduino:samd:nano_33_iot # samd21 - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esm32 - - esp32:esp32:esp32s2 # esm32s2 + - esp32:esp32:esp32 # esp32 + - esp32:esp32:esp32s2 # esp32s2 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - arduino:mbed_rp2040:pico # rpi pico @@ -41,7 +41,7 @@ jobs: platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index 56914d45..ea11c6d7 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) #include "driver/ledc.h" diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f5ed6ca1..f1893932 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" From 1d634b4c9fe5099092d2c425d30f9ab642daecc1 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jan 2022 09:34:37 +0100 Subject: [PATCH 030/474] center alignement fixed --- src/drivers/hardware_specific/esp32_mcu.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f1893932..7933e13a 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -100,6 +100,9 @@ stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { // configuring high frequency pwm timer // a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +// a short tutorial for v2.0.1+ +// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html +// void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ mcpwm_config_t pwm_config; @@ -169,8 +172,8 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); - // TIMER0 has itself as sync source, route this sync source directly to sync output for the other two - mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_SYNCIN); + // Enable sync event for all timers to be the TEZ of timer0 + mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); } // function setting the high pwm frequency to the supplied pins From 25f7b0a72a6a1749d1a2219b1214ef970b3fd81a Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jan 2022 09:35:18 +0100 Subject: [PATCH 031/474] esp v2.0.2 i2c begin issue --- .../esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index c930437b..0516ede1 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -18,7 +18,7 @@ void setup() { // Normally SimpleFOC will call begin for i2c but with esp32 begin() is the only way to set pins! // It seems safe to call begin multiple times - Wire1.begin(19, 23, 400000); + Wire1.begin(19, 23, (uint32_t)400000); sensor0.init(); sensor1.init(&Wire1); From 67832d4ca8fe4ae806fb23aace850b59024e38ca Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jan 2022 09:36:16 +0100 Subject: [PATCH 032/474] readme update esp32 v2.0.2 --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index d1673651..28fa66ae 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ Therefore this is an attempt to:
  • Voltage/current limit handling bugs #118
  • Generic position and current sense classes - to implement a new sensor only implement one function
  • Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation
    • +
  • esp32 arduino package transfer to v2.0.1+
    • From ad08e63ded91e110e7df65526419268a1e4b3800 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jan 2022 10:06:22 +0100 Subject: [PATCH 033/474] contributor added --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 28fa66ae..b0724122 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ Therefore this is an attempt to:
  • Voltage/current limit handling bugs #118
  • Generic position and current sense classes - to implement a new sensor only implement one function
  • Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation
    • -
  • esp32 arduino package transfer to v2.0.1+
    • +
  • esp32 arduino package transfer to v2.0.1+ - helpful PR#149 by samguns
    • From 34c0399e130621d24a760c1ab129bc57f6d84c76 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jan 2022 10:07:26 +0100 Subject: [PATCH 034/474] joss paper badge --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index b0724122..fd870004 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,7 @@ ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. From 912bfcd12deee67fb06b47ab81b1b61299e622f1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 23 Jan 2022 16:07:22 +0100 Subject: [PATCH 035/474] driver refactoring (completely unfinished!) --- src/common/base_classes/StepperDriver.h | 5 +- .../hardware_specific/stm32g4_mcu.cpp | 4 +- src/drivers/BLDCDriver3PWM.cpp | 7 +-- src/drivers/BLDCDriver3PWM.h | 1 - src/drivers/BLDCDriver6PWM.cpp | 7 +-- src/drivers/StepperDriver2PWM.cpp | 7 +-- src/drivers/StepperDriver4PWM.cpp | 7 +-- src/drivers/hardware_api.h | 28 ++++++--- src/drivers/hardware_specific/generic_mcu.cpp | 57 ++++++++++--------- src/drivers/hardware_specific/samd_mcu.cpp | 16 +++--- src/drivers/hardware_specific/samd_mcu.h | 8 +++ src/drivers/hardware_specific/stm32_mcu.cpp | 6 ++ 12 files changed, 90 insertions(+), 63 deletions(-) diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index aa9f9e4d..1ef88edf 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,6 +1,8 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H +#include "drivers/hardware_api.h" + class StepperDriver{ public: @@ -16,7 +18,8 @@ class StepperDriver{ float voltage_limit; //!< limiting voltage set to the motor bool initialized = false; // true if driver was successfully initialized - + HardwareDriverParams params = 0; + /** * Set phase voltages to the harware * diff --git a/src/current_sense/hardware_specific/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32g4_mcu.cpp index 5b5d00ee..6fd22327 100644 --- a/src/current_sense/hardware_specific/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32g4_mcu.cpp @@ -1,5 +1,6 @@ #include "../hardware_api.h" #include "stm32g4_hal.h" +#include "Arduino.h" #if defined(STM32G4xx) #define _ADC_VOLTAGE 3.3 @@ -29,9 +30,10 @@ float _readADCVoltageInline(const int pin){ raw_adc = adcBuffer1[1]; else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 raw_adc = adcBuffer2[0]; +#ifdef PB1 else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[0]; - +#endif return raw_adc * _ADC_CONV; } // do the same for low side sensing diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 13cba6e8..e76365f6 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -56,9 +56,8 @@ int BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); - initialized = true; // TODO atm the api gives no feedback if initialization succeeded - return 0; + params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return params!=0 && params->initSuccess; } @@ -88,5 +87,5 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, params); } diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index d6a83590..aaf38c7b 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -58,7 +58,6 @@ class BLDCDriver3PWM: public BLDCDriver */ virtual void setPhaseState(int sa, int sb, int sc) override; private: - }; diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 5b2ba9d9..722e363a 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -58,9 +58,8 @@ int BLDCDriver6PWM::init() { // configure 6pwm // hardware specific function - depending on driver and mcu - int result = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); - initialized = (result==0); - return result; + params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + return params!=0 && params->initSuccess; } // Set voltage to the pwm pin @@ -76,7 +75,7 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, params); } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 469ae694..96ced47f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -79,9 +79,8 @@ int StepperDriver2PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure2PWM(pwm_frequency, pwm1, pwm2); - initialized = true; // TODO atm the api gives no feedback if initialization succeeded - return 0; + params = _configure2PWM(pwm_frequency, pwm1, pwm2); + return params!=0 && params->initSuccess; } @@ -103,5 +102,5 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); // write to hardware - _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params); } \ No newline at end of file diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index e4305e76..c3d2dcc3 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -54,9 +54,8 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); - initialized = true; // TODO atm the api gives no feedback if initialization succeeded - return 0; + params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + return params!=0 && params->initSuccess; } @@ -77,5 +76,5 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { else duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware - _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); } \ No newline at end of file diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 0b4f7c65..9846b62a 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -4,6 +4,18 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" + +struct HardwareDriverParamsBase { + int pins[6]; + long pwm_frequency; + int dead_zone; + bool initSuccess; +}; + +typedef struct HardwareDriverParamsBase* HardwareDriverParams; + + + /** * Configuring PWM frequency, resolution and alignment * - Stepper driver - 2PWM setting @@ -13,7 +25,7 @@ * @param pinA pinA bldc driver * @param pinB pinB bldc driver */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); +HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const int pinB); /** * Configuring PWM frequency, resolution and alignment @@ -25,7 +37,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); * @param pinB pinB bldc driver * @param pinC pinC bldc driver */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); +HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); /** * Configuring PWM frequency, resolution and alignment @@ -38,7 +50,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); +HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); /** * Configuring PWM frequency, resolution and alignment @@ -56,7 +68,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const * * @return 0 if config good, -1 if failed */ -int _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); +HardwareDriverParams _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); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -68,7 +80,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const * @param pinA phase A hardware pin number * @param pinB phase B hardware pin number */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); +void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params = 0); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -82,7 +94,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); * @param pinB phase B hardware pin number * @param pinC phase C hardware pin number */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverParams params = 0); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -98,7 +110,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB * @param pin2A phase 2A hardware pin number * @param pin2B phase 2B hardware pin number */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, HardwareDriverParams params = 0); /** @@ -118,6 +130,6 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in * @param pinC_l phase C low-side hardware pin number * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, HardwareDriverParams params = 0); #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index f89c0562..61b63126 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -9,23 +9,26 @@ // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - _UNUSED(pwm_frequency); - _UNUSED(pinA); - _UNUSED(pinB); - return; +__attribute__((weak)) HardwareDriverParams _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + HardwareDriverParams params = new struct DriverParamsBase { + .pins = { pinA, pinB, -1, -1, -1, -1 }, + .pwm_frequency = pwm_frequency, + .initSuccess = true + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - _UNUSED(pwm_frequency); - _UNUSED(pinA); - _UNUSED(pinB); - _UNUSED(pinC); - return; +__attribute__((weak)) HardwareDriverParams _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + HardwareDriverParams params = new struct DriverParamsBase { + .pins = { pinA, pinB, pinC, -1, -1, -1 }, + .pwm_frequency = pwm_frequency, + .initSuccess = true + }; + return params; } @@ -33,28 +36,26 @@ __attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, con // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - _UNUSED(pwm_frequency); - _UNUSED(pin1A); - _UNUSED(pin1B); - _UNUSED(pin2A); - _UNUSED(pin2B); - return; +__attribute__((weak)) HardwareDriverParams _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + HardwareDriverParams params = new struct DriverParamsBase { + .pins = { pin1A, pin1B, pin2A, pin2B, -1, -1 }, + .pwm_frequency = pwm_frequency, + .initSuccess = true + }; + return params; } // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) int _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){ - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); - _UNUSED(pinA_h); - _UNUSED(pinB_h); - _UNUSED(pinC_h); - _UNUSED(pinA_l); - _UNUSED(pinB_l); - _UNUSED(pinC_l); - return -1; +__attribute__((weak)) HardwareDriverParams _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){ + HardwareDriverParams params = new struct DriverParamsBase { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone, + .initSuccess = true + }; + return params; } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 5c0bb71e..a642fec6 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -278,7 +278,7 @@ int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguratio * @param pinA pinA bldc driver * @param pinB pinB bldc driver */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -375,7 +375,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { * @param pinB pinB bldc driver * @param pinC pinC bldc driver */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { +HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -451,7 +451,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -552,7 +552,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const * * @return 0 if config good, -1 if failed */ -int _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) { +HardwareDriverParams _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) { // we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); @@ -644,7 +644,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const * @param pinA phase A hardware pin number * @param pinB phase B hardware pin number */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { +void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params) { tccConfiguration* tccI = getTccPinConfiguration(pinA); writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); @@ -668,7 +668,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { * @param pinB phase B hardware pin number * @param pinC phase C hardware pin number */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverParams params) { tccConfiguration* tccI = getTccPinConfiguration(pinA); writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); @@ -695,7 +695,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB * @param pin2A phase 2A hardware pin number * @param pin2B phase 2B hardware pin number */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, HardwareDriverParams params){ tccConfiguration* tccI = getTccPinConfiguration(pin1A); writeSAMDDutyCycle(tccI, dc_1a); tccI = getTccPinConfiguration(pin2A); @@ -733,7 +733,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in * @param pinC_l phase C low-side hardware pin number * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, HardwareDriverParams params){ tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 8fafeaaa..b45839ca 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -80,6 +80,14 @@ struct wo_association { +struct SAMDHardwareDriverParams : public HardwareDriverParamsBase { + SAMDHardwareDriverParams(int numPins) : tccPinConfigurations[numPins] {}; + tccConfiguration tccPinConfigurations[6]; +}; + + + + #if defined(_SAMD21_) #define NUM_PIO_TIMER_PERIPHERALS 2 #elif defined(_SAMD51_)||defined(_SAME51_) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 540dd6c2..6f03ca80 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -32,6 +32,12 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) { PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + if (Instance == NP) { + Serial.print("No timer on pin "); + Serial.println(ulPin); + delay(1000); + return NP; + } uint32_t index = get_timer_index(Instance); if (HardwareTimer_Handle[index] == NULL) { From 7f02b8af3c43ee67ee4fbd457226f94e433218f4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 2 Feb 2022 00:20:42 +0100 Subject: [PATCH 036/474] driver API refactor, part 1, the easy ones --- src/common/base_classes/BLDCDriver.h | 1 + src/common/base_classes/StepperDriver.h | 2 +- src/drivers/BLDCDriver3PWM.cpp | 2 +- src/drivers/BLDCDriver6PWM.cpp | 5 +- src/drivers/StepperDriver2PWM.cpp | 2 +- src/drivers/StepperDriver4PWM.cpp | 2 +- src/drivers/hardware_api.h | 59 ++++------ .../hardware_specific/atmega2560_mcu.cpp | 63 ++++++---- .../hardware_specific/atmega328_mcu.cpp | 67 +++++++---- .../hardware_specific/atmega32u4_mcu.cpp | 69 +++++++---- src/drivers/hardware_specific/esp8266_mcu.cpp | 45 +++++--- src/drivers/hardware_specific/generic_mcu.cpp | 73 +++++------- src/drivers/hardware_specific/rp2040_mcu.cpp | 109 +++++++++++------- src/drivers/hardware_specific/samd_mcu.cpp | 94 ++++++++------- src/drivers/hardware_specific/samd_mcu.h | 9 +- src/drivers/hardware_specific/teensy_mcu.cpp | 45 +++++--- 16 files changed, 371 insertions(+), 276 deletions(-) diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 3e220d2e..d40929ad 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -23,6 +23,7 @@ class BLDCDriver{ float dc_c; //!< currently set duty cycle on phaseC bool initialized = false; // true if driver was successfully initialized + void* params = 0; // pointer to hardware specific parameters of driver /** * Set phase voltages to the harware diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 1ef88edf..2006fc8c 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -18,7 +18,7 @@ class StepperDriver{ float voltage_limit; //!< limiting voltage set to the motor bool initialized = false; // true if driver was successfully initialized - HardwareDriverParams params = 0; + void* params = 0; // pointer to hardware specific parameters of driver /** * Set phase voltages to the harware diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index e76365f6..00714767 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -57,7 +57,7 @@ int BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); - return params!=0 && params->initSuccess; + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 722e363a..2b039d79 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -59,9 +59,10 @@ int BLDCDriver6PWM::init() { // configure 6pwm // hardware specific function - depending on driver and mcu params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); - return params!=0 && params->initSuccess; + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } + // Set voltage to the pwm pin void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver @@ -75,7 +76,7 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, params); + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, params); } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 96ced47f..bec6819b 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -80,7 +80,7 @@ int StepperDriver2PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure2PWM(pwm_frequency, pwm1, pwm2); - return params!=0 && params->initSuccess; + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index c3d2dcc3..a2fb4c77 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -55,7 +55,7 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); - return params!=0 && params->initSuccess; + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 9846b62a..84eb5999 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -4,16 +4,13 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) -struct HardwareDriverParamsBase { - int pins[6]; - long pwm_frequency; - int dead_zone; - bool initSuccess; -}; - -typedef struct HardwareDriverParamsBase* HardwareDriverParams; - +typedef struct GenericDriverParams { + int pins[6]; + long pwm_frequency; + float dead_zone; +} GenericDriverParams; /** @@ -24,8 +21,10 @@ typedef struct HardwareDriverParamsBase* HardwareDriverParams; * @param pwm_frequency - frequency in hertz - if applicable * @param pinA pinA bldc driver * @param pinB pinB bldc driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const int pinB); +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB); /** * Configuring PWM frequency, resolution and alignment @@ -36,8 +35,10 @@ HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const in * @param pinA pinA bldc driver * @param pinB pinB bldc driver * @param pinC pinC bldc driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); /** * Configuring PWM frequency, resolution and alignment @@ -49,8 +50,10 @@ HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const in * @param pin1B pin1B stepper driver * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); /** * Configuring PWM frequency, resolution and alignment @@ -66,9 +69,9 @@ HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const i * @param pinC_h pinA high-side bldc driver * @param pinC_l pinA low-side bldc driver * - * @return 0 if config good, -1 if failed + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -HardwareDriverParams _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); +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); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -77,10 +80,9 @@ HardwareDriverParams _configure6PWM(long pwm_frequency, float dead_zone, const i * * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params = 0); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -90,11 +92,9 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params = * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] * @param dc_c duty cycle phase C [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - * @param pinC phase C hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverParams params = 0); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -105,12 +105,9 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverPara * @param dc_1b duty cycle phase 1B [0, 1] * @param dc_2a duty cycle phase 2A [0, 1] * @param dc_2b duty cycle phase 2B [0, 1] - * @param pin1A phase 1A hardware pin number - * @param pin1B phase 1B hardware pin number - * @param pin2A phase 2A hardware pin number - * @param pin2B phase 2B hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, HardwareDriverParams params = 0); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params); /** @@ -121,15 +118,9 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, Ha * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] * @param dc_c duty cycle phase C [0, 1] - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - * @param pinA_h phase A high-side hardware pin number - * @param pinA_l phase A low-side hardware pin number - * @param pinB_h phase B high-side hardware pin number - * @param pinB_l phase B low-side hardware pin number - * @param pinC_h phase C high-side hardware pin number - * @param pinC_l phase C low-side hardware pin number + * @param params the driver parameters * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, HardwareDriverParams params = 0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params); #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 2d3c0305..cf71a949 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -28,64 +28,79 @@ void _pinHighFrequency(const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + 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 -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -122,14 +137,20 @@ int _configureComplementaryPair(int pinH, int pinL) { // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -int _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) { +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) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + .dead_zone = dead_zone + }; + return params; } // function setting the @@ -149,10 +170,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 2c64ee64..8bf58835 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -21,68 +21,81 @@ void _pinHighFrequency(const int pin){ // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328 -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - _UNUSED(pwm_frequency); +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic // supports Arudino/ATmega328 -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - _UNUSED(pwm_frequency); +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + 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 -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic // supports Arudino/ATmega328 -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -118,16 +131,20 @@ int _configureComplementaryPair(int pinH, int pinL) { // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -int _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) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); +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) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + .dead_zone = dead_zone + }; + return params; } // function setting the @@ -147,10 +164,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 5a8abe2e..22b6c656 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -31,64 +31,79 @@ void _pinHighFrequency(const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + 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 -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -134,17 +149,21 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _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) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); +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) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured - + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + .dead_zone = dead_zone + }; + return params; +} // function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) @@ -163,10 +182,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index ccc31aa2..df8ae380 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -15,63 +15,78 @@ 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 _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // 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) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + 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 -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 61b63126..b8191b01 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -9,11 +9,10 @@ // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) HardwareDriverParams _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - HardwareDriverParams params = new struct DriverParamsBase { - .pins = { pinA, pinB, -1, -1, -1, -1 }, - .pwm_frequency = pwm_frequency, - .initSuccess = true +__attribute__((weak)) void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency }; return params; } @@ -22,11 +21,10 @@ __attribute__((weak)) HardwareDriverParams _configure2PWM(long pwm_frequency,con // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) HardwareDriverParams _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - HardwareDriverParams params = new struct DriverParamsBase { - .pins = { pinA, pinB, pinC, -1, -1, -1 }, - .pwm_frequency = pwm_frequency, - .initSuccess = true +__attribute__((weak)) void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency }; return params; } @@ -36,11 +34,10 @@ __attribute__((weak)) HardwareDriverParams _configure3PWM(long pwm_frequency,con // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) HardwareDriverParams _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - HardwareDriverParams params = new struct DriverParamsBase { - .pins = { pin1A, pin1B, pin2A, pin2B, -1, -1 }, - .pwm_frequency = pwm_frequency, - .initSuccess = true +__attribute__((weak)) void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin1B, pin2A, pin2B }, + .pwm_frequency = pwm_frequency }; return params; } @@ -48,61 +45,49 @@ __attribute__((weak)) HardwareDriverParams _configure4PWM(long pwm_frequency,con // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) HardwareDriverParams _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){ - HardwareDriverParams params = new struct DriverParamsBase { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone, - .initSuccess = true - }; - return params; +__attribute__((weak)) 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){ + return SIMPLEFOC_DRIVER_INIT_FAILED; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + 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 -__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ _UNUSED(dc_a); _UNUSED(dc_b); _UNUSED(dc_c); - _UNUSED(dead_zone); - _UNUSED(pinA_h); - _UNUSED(pinB_h); - _UNUSED(pinC_h); - _UNUSED(pinA_l); - _UNUSED(pinB_l); - _UNUSED(pinC_l); - return; + _UNUSED(params); } diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 7be8faaf..2c127d71 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -19,6 +19,14 @@ +typedef struct RP2040DriverParams { + int pins[6]; + uint slice[6]; + uint chan[6]; + long pwm_frequency; + float dead_zone; +} RP2040DriverParams; + // until I can figure out if this can be quickly read from some register, keep it here. // it also serves as a marker for what slices are already used. @@ -27,10 +35,13 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert = false) { +void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); + params->pins[index] = pin; + params->slice[index] = slice; + params->chan[index] = chan; pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get pwm_set_phase_correct(slice, true); uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; @@ -61,7 +72,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert = false) { void syncSlices() { - for (int i=0;ipwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, false, params, 0); + setupPWM(pinB, pwm_frequency, false, params, 1); syncSlices(); + return params; } -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { - setupPWM(pinA, pwm_frequency); - setupPWM(pinB, pwm_frequency); - setupPWM(pinC, pwm_frequency); +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, false, params, 0); + setupPWM(pinB, pwm_frequency, false, params, 1); + setupPWM(pinC, pwm_frequency, false, params, 2); syncSlices(); + return params; } -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - setupPWM(pin1A, pwm_frequency); - setupPWM(pin1B, pwm_frequency); - setupPWM(pin2A, pwm_frequency); - setupPWM(pin2B, pwm_frequency); +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + setupPWM(pin1A, pwm_frequency, false, params, 0); + setupPWM(pin1B, pwm_frequency, false, params, 1); + setupPWM(pin2A, pwm_frequency, false, params, 2); + setupPWM(pin2B, pwm_frequency, false, params, 3); syncSlices(); + return params; } -int _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) { +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) { // non-PIO solution... - setupPWM(pinA_h, pwm_frequency); - setupPWM(pinB_h, pwm_frequency); - setupPWM(pinC_h, pwm_frequency); - setupPWM(pinA_l, pwm_frequency, true); - setupPWM(pinB_l, pwm_frequency, true); - setupPWM(pinC_l, pwm_frequency, true); + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + params->dead_zone = dead_zone; + setupPWM(pinA_h, pwm_frequency, false, params, 0); + setupPWM(pinB_h, pwm_frequency, false, params, 2); + setupPWM(pinC_h, pwm_frequency, false, params, 4); + setupPWM(pinA_l, pwm_frequency, true, params, 1); + setupPWM(pinB_l, pwm_frequency, true, params, 3); + setupPWM(pinC_l, pwm_frequency, true, params, 5); syncSlices(); - return 0; + return params; } -void writeDutyCycle(float val, int pin) { - uint slice = pwm_gpio_to_slice_num(pin); - uint chan = pwm_gpio_to_channel(pin); +void writeDutyCycle(float val, uint slice, uint chan) { pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val); } @@ -123,26 +144,26 @@ void writeDutyCycle(float val, int pin) { -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { - writeDutyCycle(dc_a, pinA); - writeDutyCycle(dc_b, pinB); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); } -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { - writeDutyCycle(dc_a, pinA); - writeDutyCycle(dc_b, pinB); - writeDutyCycle(dc_c, pinC); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); } -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B) { - writeDutyCycle(dc_1a, pin1A); - writeDutyCycle(dc_1b, pin1B); - writeDutyCycle(dc_2a, pin2A); - writeDutyCycle(dc_2b, pin2B); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { + writeDutyCycle(dc_1a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_1b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_2a, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + writeDutyCycle(dc_2b, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); } inline float swDti(float val, float dt) { @@ -151,13 +172,13 @@ inline float swDti(float val, float dt) { return ret; } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { - writeDutyCycle(dc_a, pinA_h); - writeDutyCycle(swDti(dc_a, dead_zone), pinA_l); - writeDutyCycle(dc_b, pinB_h); - writeDutyCycle(swDti(dc_b,dead_zone), pinB_l); - writeDutyCycle(dc_c, pinC_h); - writeDutyCycle(swDti(dc_c,dead_zone), pinC_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(swDti(dc_a, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + writeDutyCycle(swDti(dc_b, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]); + writeDutyCycle(swDti(dc_c, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]); } #endif diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index a642fec6..02f27d9d 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -278,7 +278,7 @@ int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguratio * @param pinA pinA bldc driver * @param pinB pinB bldc driver */ -HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -289,7 +289,7 @@ HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const in #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)), @@ -329,7 +329,11 @@ HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const in SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif - return; // Someone with a stepper-setup who can test it? + SAMDHardwareDriverParams* params = new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; // Someone with a stepper-setup who can test it? + return params; } @@ -375,7 +379,7 @@ HardwareDriverParams _configure2PWM(long pwm_frequency, const int pinA, const in * @param pinB pinB bldc driver * @param pinC pinC bldc driver */ -HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -386,7 +390,7 @@ HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const in #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)), @@ -431,6 +435,11 @@ HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const in SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB), getTccPinConfiguration(pinC) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; + } @@ -451,7 +460,7 @@ HardwareDriverParams _configure3PWM(long pwm_frequency, const int pinA, const in * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver */ -HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -462,7 +471,7 @@ HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const i #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, getPeripheralOfPermutation(compatibility, 0)), @@ -512,7 +521,10 @@ HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const i SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif - return; // Someone with a stepper-setup who can test it? + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pin1A), getTccPinConfiguration(pin1B), getTccPinConfiguration(pin2A), getTccPinConfiguration(pin2B) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; } @@ -552,7 +564,7 @@ HardwareDriverParams _configure4PWM(long pwm_frequency, const int pin1A, const i * * @return 0 if config good, -1 if failed */ -HardwareDriverParams _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) { +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) { // we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); @@ -565,7 +577,7 @@ HardwareDriverParams _configure6PWM(long pwm_frequency, float dead_zone, const i #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif - return -1; + return SIMPLEFOC_DRIVER_INIT_FAILED; } } @@ -595,7 +607,7 @@ HardwareDriverParams _configure6PWM(long pwm_frequency, float dead_zone, const i allAttached |= attachTCC(pinCh); allAttached |= attachTCC(pinCl); if (!allAttached) - return -1; + return SIMPLEFOC_DRIVER_INIT_FAILED; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif @@ -628,7 +640,11 @@ HardwareDriverParams _configure6PWM(long pwm_frequency, float dead_zone, const i SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif - return 0; + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA_h), getTccPinConfiguration(pinA_l), getTccPinConfiguration(pinB_h), getTccPinConfiguration(pinB_l), getTccPinConfiguration(pinC_h), getTccPinConfiguration(pinC_l) }, + .pwm_frequency = (uint32_t)pwm_frequency, + .dead_zone = dead_zone, + }; } @@ -644,11 +660,9 @@ HardwareDriverParams _configure6PWM(long pwm_frequency, float dead_zone, const i * @param pinA phase A hardware pin number * @param pinB phase B hardware pin number */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI, dc_b); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b); return; } @@ -668,13 +682,10 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, HardwareDriverParams params) { * @param pinB phase B hardware pin number * @param pinC phase C hardware pin number */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverParams params) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI, dc_b); - tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI, dc_c); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC, void* params) { + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_c); return; } @@ -695,15 +706,11 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, HardwareDriverPara * @param pin2A phase 2A hardware pin number * @param pin2B phase 2B hardware pin number */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, HardwareDriverParams params){ - tccConfiguration* tccI = getTccPinConfiguration(pin1A); - writeSAMDDutyCycle(tccI, dc_1a); - tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI, dc_2a); - tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI, dc_1b); - tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI, dc_2b); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B, void* params){ + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_1a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_1b); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_2a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[3], dc_2b); return; } @@ -733,12 +740,13 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, Ha * @param pinC_l phase C low-side hardware pin number * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, HardwareDriverParams params){ - tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); - tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l, float dead_zone, void* params){ + tccConfiguration* tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0]; + tccConfiguration* tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1]; + uint32_t pwm_res =((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0]->pwm_res; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... - float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_a+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); // TODO resolution!!! if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_a); writeSAMDDutyCycle(tcc2, ls); @@ -746,10 +754,10 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, H else writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly - tcc1 = getTccPinConfiguration(pinB_h); - tcc2 = getTccPinConfiguration(pinB_l); + tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2]; + tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[3]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_b+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_b); writeSAMDDutyCycle(tcc2, ls); @@ -757,10 +765,10 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, H else writeSAMDDutyCycle(tcc1, dc_b); - tcc1 = getTccPinConfiguration(pinC_h); - tcc2 = getTccPinConfiguration(pinC_l); + tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[4]; + tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[5]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_c+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_c); writeSAMDDutyCycle(tcc2, ls); diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index b45839ca..78f53664 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -80,10 +80,11 @@ struct wo_association { -struct SAMDHardwareDriverParams : public HardwareDriverParamsBase { - SAMDHardwareDriverParams(int numPins) : tccPinConfigurations[numPins] {}; - tccConfiguration tccPinConfigurations[6]; -}; +typedef struct SAMDHardwareDriverParams { + tccConfiguration* tccPinConfigurations[6]; + uint32_t pwm_frequency; + float dead_zone; +} SAMDHardwareDriverParams; diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 67bb0ad1..93331252 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -15,63 +15,78 @@ 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 _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // 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) { +void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + 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 -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file From 84269cd081abb400856725539d65e6ebab9636f8 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 3 Feb 2022 11:30:48 +0100 Subject: [PATCH 037/474] commander refactorred and separated motion control into a new callback --- README.md | 6 +- .../single_full_control_example.ino | 4 +- .../find_current_sense_gains.ino | 176 +++++++++++++ .../find_current_sense_gains.ino | 173 ++++++++++++ .../find_pole_pairs_number.ino | 1 + .../find_pole_pairs_number.ino | 1 + keywords.txt | 1 + src/communication/Commander.cpp | 247 +++++++++++------- src/communication/Commander.h | 61 ++++- 9 files changed, 564 insertions(+), 106 deletions(-) create mode 100644 examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino create mode 100644 examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino diff --git a/README.md b/README.md index fd870004..105628fd 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,11 @@ Therefore this is an attempt to:

    FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

    • Sensor class init bugfix #121
      • -
    • Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once
      • +
    • Added the new motion control interface to the commander +
        +
      • New target setting - possible to set the position, velocity and torque target at once
        • +
      • Separated the motion control interface from full motor callback - only motion control and torque control type, enable disable and target setting
        • +
    • NRF52 series mcus support by @Polyphe
    • Voltage/current limit handling bugs #118
    • Generic position and current sense classes - to implement a new sensor only implement one function
      • diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 2d6c3ca6..c1b224e6 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -17,7 +17,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +void doMotion(char* cmd){ command.motion(&motor, cmd); } // void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -74,7 +74,7 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('T', doTarget, "target"); + command.add('T', doMotion, "motion control"); // command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) diff --git a/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino b/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino new file mode 100644 index 00000000..2ac704e3 --- /dev/null +++ b/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino @@ -0,0 +1,176 @@ +/** + * Utility arduino sketch which finds pole pair number of the motor + * + * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value. + * + * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * + * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. + */ +#include + +// BLDC motor instance +BLDCMotor motor = BLDCMotor(10); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// Encoder(int encA, int encB , int cpr, int index) +Encoder encoder = Encoder(2, 3, 2048); +// interrupt routine intialisation +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// current sensor +// shunt resistor value +// gain value +// pins phase A,B, (C optional) +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); + +void setup() { + + // initialise encoder hardware + encoder.init(); + // hardware interrupt enable + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // power supply voltage + // default 12V + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // initialize motor + motor.init(); + // monitoring port + Serial.begin(115200); + + // initialise the current sensing + current_sense.init(); + + // pole pairs calculation routine + Serial.println("Pole pairs (PP) estimator"); + Serial.println("-\n"); + + float pp_search_voltage = 4; // maximum power_supply_voltage/2 + float pp_search_angle = 8*M_PI; // search electrical angle to turn + float pp_vel_limit = 1; + + // move motor to the electrical angle 0 + motor.controller = MotionControlType::angle_openloop; + motor.voltage_limit = pp_search_voltage; + motor.velocity_limit = pp_vel_limit; + motor.move(0); + _delay(1000); + + // move the motor slowly to the electrical angle pp_search_angle + float a_max=0,b_max=0,c_max=0; + while(motor_angle <= pp_search_angle){ + motor.move(pp_search_angle); + PhaseCurrent_s c = current_sense.getPhaseCurrents(); + a_max = fabs(c.a) > a_max ? fabs(c.a) : a_max; + b_max = fabs(c.b) > b_max ? fabs(c.b) : b_max; + c_max = fabs(c.c) > c_max ? fabs(c.c) : c_max; + } + _delay(1000); + + Serial.print(a_max); + Serial.print("\t"); + Serial.print(b_max); + Serial.print("\t"); + Serial.println(b_max); + + return; + // calculate the pole pair number + int pp = round((pp_search_angle)/(angle_end-angle_begin)); + + Serial.print(F("Estimated PP : ")); + Serial.println(pp); + Serial.println(F("PP = Electrical angle / Encoder angle ")); + Serial.print(pp_search_angle*180/M_PI); + Serial.print("/"); + Serial.print((angle_end-angle_begin)*180/M_PI); + Serial.print(" = "); + Serial.println((pp_search_angle)/(angle_end-angle_begin)); + Serial.println(); + + + // a bit of monitoring the result + if(pp <= 0 ){ + Serial.println(F("PP number cannot be negative")); + Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration.")); + return; + }else if(pp > 30){ + Serial.println(F("PP number very high, possible error.")); + }else{ + Serial.println(F("If PP is estimated well your motor should turn now!")); + Serial.println(F(" - If it is not moving try to relaunch the program!")); + Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); + } + + + // set FOC loop to be used + motor.controller = MotionControlType::torque; + // set the pole pair number to the motor + motor.pole_pairs = pp; + //align encoder and start FOC + motor.initFOC(); + _delay(1000); + + Serial.println(F("\n Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); +} + +// uq voltage +float target_voltage = 2; + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_voltage); + + // communicate with the user + serialReceiveUserCommand(); +} + + +// utility function enabling serial communication with the user to set the target values +// this function can be implemented in serialEvent function as well +void serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + + // change the motor target + target_voltage = received_chars.toFloat(); + Serial.print("Target voltage: "); + Serial.println(target_voltage); + + // reset the command buffer + received_chars = ""; + } + } +} \ No newline at end of file diff --git a/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino b/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino new file mode 100644 index 00000000..f042661f --- /dev/null +++ b/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino @@ -0,0 +1,173 @@ +/** + * Utility arduino sketch which finds pole pair number of the motor + * + * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin. + * + * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * + * If the code calculates negative pole pair number please invert your motor connector. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. + */ +#include + +// BLDC motor instance +// its important to put pole pairs number as 1!!! +BLDCMotor motor = BLDCMotor(1); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor instance +// its important to put pole pairs number as 1!!! +//StepperMotor motor = StepperMotor(1); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); +// magnetic sensor instance - I2C +//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); +// magnetic sensor instance - analog output +// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // power supply voltage + // default 12V + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // initialize motor hardware + motor.init(); + + // monitoring port + Serial.begin(115200); + + // pole pairs calculation routine + Serial.println("Pole pairs (PP) estimator"); + Serial.println("-\n"); + + float pp_search_voltage = 4; // maximum power_supply_voltage/2 + float pp_search_angle = 6*M_PI; // search electrical angle to turn + + // move motor to the electrical angle 0 + motor.controller = MotionControlType::angle_openloop; + motor.voltage_limit=pp_search_voltage; + motor.move(0); + _delay(1000); + // read the sensor angle + sensor.update(); + float angle_begin = sensor.getAngle(); + _delay(50); + + // move the motor slowly to the electrical angle pp_search_angle + float motor_angle = 0; + while(motor_angle <= pp_search_angle){ + motor_angle += 0.01f; + sensor.update(); // keep track of the overflow + motor.move(motor_angle); + _delay(1); + } + _delay(1000); + // read the sensor value for 180 + sensor.update(); + float angle_end = sensor.getAngle(); + _delay(50); + // turn off the motor + motor.move(0); + _delay(1000); + + // calculate the pole pair number + int pp = round((pp_search_angle)/(angle_end-angle_begin)); + + Serial.print(F("Estimated PP : ")); + Serial.println(pp); + Serial.println(F("PP = Electrical angle / Encoder angle ")); + Serial.print(pp_search_angle*180/M_PI); + Serial.print(F("/")); + Serial.print((angle_end-angle_begin)*180/M_PI); + Serial.print(F(" = ")); + Serial.println((pp_search_angle)/(angle_end-angle_begin)); + Serial.println(); + + + // a bit of monitoring the result + if(pp <= 0 ){ + Serial.println(F("PP number cannot be negative")); + Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration.")); + return; + }else if(pp > 30){ + Serial.println(F("PP number very high, possible error.")); + }else{ + Serial.println(F("If PP is estimated well your motor should turn now!")); + Serial.println(F(" - If it is not moving try to relaunch the program!")); + Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); + } + + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + // set the pole pair number to the motor + motor.pole_pairs = pp; + //align sensor and start FOC + motor.initFOC(); + _delay(1000); + + Serial.println(F("\n Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); +} + +// uq voltage +float target_voltage = 2; + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_voltage); + + // communicate with the user + serialReceiveUserCommand(); +} + + +// utility function enabling serial communication with the user to set the target values +// this function can be implemented in serialEvent function as well +void serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + + // change the motor target + target_voltage = received_chars.toFloat(); + Serial.print("Target voltage: "); + Serial.println(target_voltage); + + // reset the command buffer + received_chars = ""; + } + } +} diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index c5dfbf43..d2aab9e8 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -73,6 +73,7 @@ void setup() { while(motor_angle <= pp_search_angle){ motor_angle += 0.01f; motor.move(motor_angle); + _delay(1); } _delay(1000); // read the encoder value for 180 diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index 0b07c772..f042661f 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -73,6 +73,7 @@ void setup() { motor_angle += 0.01f; sensor.update(); // keep track of the overflow motor.move(motor_angle); + _delay(1); } _delay(1000); // read the sensor value for 180 diff --git a/keywords.txt b/keywords.txt index a01ff880..add21dce 100644 --- a/keywords.txt +++ b/keywords.txt @@ -122,6 +122,7 @@ pullup KEYWORD2 quadrature KEYWORD2 foc_modulation KEYWORD2 target KEYWORD2 +motion KEYWORD2 pwm_frequency KEYWORD2 dead_zone KEYWORD2 gain_a KEYWORD2 diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 42dab7c3..1d9b4e59 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -102,7 +102,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // if target setting if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){ printVerbose(F("Target: ")); - motor->target = atof(user_command); + target(motor, user_command); println(motor->target); return; } @@ -177,65 +177,9 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MOTION_TYPE: - printVerbose(F("Motion:")); - switch(sub_cmd){ - case SCMD_DOWNSAMPLE: - printVerbose(F(" downsample: ")); - if(!GET) motor->motion_downsample = value; - println((int)motor->motion_downsample); - break; - default: - // change control type - if(!GET && value >= 0 && (int)value < 5) // if set command - motor->controller = (MotionControlType)value; - switch(motor->controller){ - case MotionControlType::torque: - println(F("torque")); - break; - case MotionControlType::velocity: - println(F("vel")); - break; - case MotionControlType::angle: - println(F("angle")); - break; - case MotionControlType::velocity_openloop: - println(F("vel open")); - break; - case MotionControlType::angle_openloop: - println(F("angle open")); - break; - } - break; - } - break; case CMD_TORQUE_TYPE: - // change control type - printVerbose(F("Torque: ")); - if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command - motor->torque_controller = (TorqueControlType)value; - switch(motor->torque_controller){ - case TorqueControlType::voltage: - println(F("volt")); - // change the velocity control limits if necessary - if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit; - break; - case TorqueControlType::dc_current: - println(F("dc curr")); - // change the velocity control limits if necessary - motor->PID_velocity.limit = motor->current_limit; - break; - case TorqueControlType::foc_current: - println(F("foc curr")); - // change the velocity control limits if necessary - motor->PID_velocity.limit = motor->current_limit; - break; - } - break; case CMD_STATUS: - // enable/disable - printVerbose(F("Status: ")); - if(!GET) (bool)value ? motor->enable() : motor->disable(); - println(motor->enabled); + motion(motor, &user_command[0]); break; case CMD_PWMMOD: // PWM modulation change @@ -384,6 +328,80 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } } +void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ + char cmd = user_cmd[0]; + char sub_cmd = user_cmd[1]; + bool GET = isSentinel(user_cmd[1]); + float value = atof(&user_cmd[(sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1]); + + switch(cmd){ + case CMD_MOTION_TYPE: + printVerbose(F("Motion:")); + switch(sub_cmd){ + case SCMD_DOWNSAMPLE: + printVerbose(F(" downsample: ")); + if(!GET) motor->motion_downsample = value; + println((int)motor->motion_downsample); + break; + default: + // change control type + if(!GET && value >= 0 && (int)value < 5) // if set command + motor->controller = (MotionControlType)value; + switch(motor->controller){ + case MotionControlType::torque: + println(F("torque")); + break; + case MotionControlType::velocity: + println(F("vel")); + break; + case MotionControlType::angle: + println(F("angle")); + break; + case MotionControlType::velocity_openloop: + println(F("vel open")); + break; + case MotionControlType::angle_openloop: + println(F("angle open")); + break; + } + break; + } + break; + case CMD_TORQUE_TYPE: + // change control type + printVerbose(F("Torque: ")); + if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command + motor->torque_controller = (TorqueControlType)value; + switch(motor->torque_controller){ + case TorqueControlType::voltage: + println(F("volt")); + // change the velocity control limits if necessary + if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit; + break; + case TorqueControlType::dc_current: + println(F("dc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; + break; + case TorqueControlType::foc_current: + println(F("foc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; + break; + } + break; + case CMD_STATUS: + // enable/disable + printVerbose(F("Status: ")); + if(!GET) (bool)value ? motor->enable() : motor->disable(); + println(motor->enabled); + break; + default: + target(motor, user_cmd, separator); + break; + } +} + void Commander::pid(PIDController* pid, char* user_cmd){ char cmd = user_cmd[0]; bool GET = isSentinel(user_cmd[1]); @@ -445,56 +463,89 @@ void Commander::scalar(float* value, char* user_cmd){ } -void Commander::target(FOCMotor* motor, char* user_cmd){ - bool GET = isSentinel(user_cmd[0]); - if(!GET){ - float pos, vel, torque; - switch(motor->controller){ - case MotionControlType::torque: // setting torque target - torque= atof(strtok (user_cmd," ")); - motor->target = torque; - break; - case MotionControlType::velocity: // setting velocity target + torque limit - vel= atof(strtok (user_cmd," ")); - torque= atof(strtok (NULL," ")); - motor->target = vel; +void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){ + // if no values sent + if(isSentinel(user_cmd[0])) return; + + float pos, vel, torque; + char* next_value; + switch(motor->controller){ + case MotionControlType::torque: // setting torque target + torque = atof(strtok (user_cmd, separator)); + motor->target = torque; + break; + case MotionControlType::velocity: // setting velocity target + torque limit + // set the target + vel= atof(strtok (user_cmd, separator)); + motor->target = vel; + + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value){ + torque = atof(next_value); motor->PID_velocity.limit = torque; // torque command can be voltage or current if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; else motor->current_limit = torque; - break; - case MotionControlType::angle: // setting angle target + torque, velocity limit - pos= atof(strtok (user_cmd," ")); - vel= atof(strtok (NULL," ")); - torque= atof(strtok (NULL," ")); - motor->target = pos; + } + break; + case MotionControlType::angle: // setting angle target + torque, velocity limit + // setting the target position + pos= atof(strtok (user_cmd, separator)); + motor->target = pos; + + // allow for setting only the target position without chaning the velocity/torque limits + next_value = strtok (NULL, separator); + if( next_value ){ + vel = atof(next_value); motor->velocity_limit = vel; motor->P_angle.limit = vel; - motor->PID_velocity.limit = torque; - // torque command can be voltage or current - if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; - else motor->current_limit = torque; - break; - case MotionControlType::velocity_openloop: // setting velocity target + torque limit - vel= atof(strtok (user_cmd," ")); - torque= atof(strtok (NULL," ")); - motor->target = vel; + + // allow for setting only the target position and velocity limit without the torque limit + next_value = strtok (NULL, separator); + if( next_value ){ + torque= atof(next_value); + motor->PID_velocity.limit = torque; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + } + break; + case MotionControlType::velocity_openloop: // setting velocity target + torque limit + // set the target + vel= atof(strtok (user_cmd, separator)); + motor->target = vel; + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value ){ + torque = atof(next_value); // torque command can be voltage or current if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; else motor->current_limit = torque; - break; - case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit - pos= atof(strtok (user_cmd," ")); - vel= atof(strtok (NULL," ")); - torque= atof(strtok (NULL," ")); - motor->target = pos; + } + break; + case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit + // set the target + pos= atof(strtok (user_cmd, separator)); + motor->target = pos; + + // allow for setting only the target position without chaning the velocity/torque limits + next_value = strtok (NULL, separator); + if( next_value ){ + vel = atof(next_value); motor->velocity_limit = vel; - // torque command can be voltage or current - if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; - else motor->current_limit = torque; - break; - } - } + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value ){ + torque = atof(next_value); + // torque command can be voltage or current + if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + } + break; + } //println(*value); } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 924d7ae5..3e653945 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -100,9 +100,13 @@ class Commander Stream* com_port = nullptr; //!< Serial terminal variable if provided char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) + /** * * FOC motor (StepperMotor and BLDCMotor) command interface + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) * 'D' - D current PID controller & LPF (see function pid and lpf for commands) @@ -149,6 +153,9 @@ class Commander /** * Low pass fileter command interface + * @param lpf - LowPassFilter instance + * @param user_cmd - the string command + * * - It only has one property - filtering time constant Tf * - It can be get by sending 'F' * - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01) @@ -156,6 +163,9 @@ class Commander void lpf(LowPassFilter* lpf, char* user_cmd); /** * PID controller command interface + * @param pid - PIDController instance + * @param user_cmd - the string command + * * - It has several paramters (the letters can be changed in the commands.h file) * - P gain - 'P' * - I gain - 'I' @@ -168,6 +178,9 @@ class Commander void pid(PIDController* pid, char* user_cmd); /** * Float variable scalar command interface + * @param value - float variable pointer + * @param user_cmd - the string command + * * - It only has one property - one float value * - It can be get by sending an empty string '\n' * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) @@ -175,15 +188,53 @@ class Commander void scalar(float* value, char* user_cmd); /** * Target setting interface, enables setting the target and limiting variables at once. - * The valeus are sent separated by a space. ex. P2.34 70 2 + * The values are sent separated by a separator specified as the third argument. The default separator is the space. + * + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * @param separator - the string separator in between target and limit values, default is space - " " + * + * Example: P2.34 70 2 * `P` is the user defined command, `2.34` is the target angle `70` is the target * velocity and `2` is the desired max current. + * * It depends of the motion control mode: - * - torque : torque (ex. P2.5) - * - velocity : velocity torque (ex.P10 2.5) - * - angle : angle velocity torque (ex.P3.5 10 2.5) + * - torque : torque (ex. P2.5) + * - velocity : velocity torque (ex.P10 2.5 or P10 to only chanage the target witout limits) + * - angle : angle velocity torque (ex.P3.5 10 2.5 or P3.5 to only chanage the target witout limits) + */ + void target(FOCMotor* motor, char* user_cmd, char* separator = " "); + + /** + * FOC motor (StepperMotor and BLDCMotor) motion control interfaces + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * @param separator - the string separator in between target and limit values, default is space - " " + * + * Commands: + * 'C' - Motion control type config + * sub-commands: + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type + * sub-commands: + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) + * sub-commands: + * '0' - enable + * '1' - disable + * '' - Target setting interface + * Depends of the motion control mode: + * - torque : torque (ex. M2.5) + * - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits) + * - angle (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits) */ - void target(FOCMotor* motor, char* user_cmd); + void motion(FOCMotor* motor, char* user_cmd, char* separator = " "); + private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number From 42cf4b4a9f01273511f2c67a2a8be16d4049f722 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 3 Feb 2022 11:32:16 +0100 Subject: [PATCH 038/474] remove calib current --- .../find_current_sense_gains.ino | 176 ------------------ .../find_current_sense_gains.ino | 173 ----------------- 2 files changed, 349 deletions(-) delete mode 100644 examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino delete mode 100644 examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino diff --git a/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino b/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino deleted file mode 100644 index 2ac704e3..00000000 --- a/examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino +++ /dev/null @@ -1,176 +0,0 @@ -/** - * Utility arduino sketch which finds pole pair number of the motor - * - * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value. - * - * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * - * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. - */ -#include - -// BLDC motor instance -BLDCMotor motor = BLDCMotor(10); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); - -// Encoder(int encA, int encB , int cpr, int index) -Encoder encoder = Encoder(2, 3, 2048); -// interrupt routine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -// current sensor -// shunt resistor value -// gain value -// pins phase A,B, (C optional) -InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); - -void setup() { - - // initialise encoder hardware - encoder.init(); - // hardware interrupt enable - encoder.enableInterrupts(doA, doB); - // link the motor to the sensor - motor.linkSensor(&encoder); - - // power supply voltage - // default 12V - driver.voltage_power_supply = 12; - driver.init(); - motor.linkDriver(&driver); - - // initialize motor - motor.init(); - // monitoring port - Serial.begin(115200); - - // initialise the current sensing - current_sense.init(); - - // pole pairs calculation routine - Serial.println("Pole pairs (PP) estimator"); - Serial.println("-\n"); - - float pp_search_voltage = 4; // maximum power_supply_voltage/2 - float pp_search_angle = 8*M_PI; // search electrical angle to turn - float pp_vel_limit = 1; - - // move motor to the electrical angle 0 - motor.controller = MotionControlType::angle_openloop; - motor.voltage_limit = pp_search_voltage; - motor.velocity_limit = pp_vel_limit; - motor.move(0); - _delay(1000); - - // move the motor slowly to the electrical angle pp_search_angle - float a_max=0,b_max=0,c_max=0; - while(motor_angle <= pp_search_angle){ - motor.move(pp_search_angle); - PhaseCurrent_s c = current_sense.getPhaseCurrents(); - a_max = fabs(c.a) > a_max ? fabs(c.a) : a_max; - b_max = fabs(c.b) > b_max ? fabs(c.b) : b_max; - c_max = fabs(c.c) > c_max ? fabs(c.c) : c_max; - } - _delay(1000); - - Serial.print(a_max); - Serial.print("\t"); - Serial.print(b_max); - Serial.print("\t"); - Serial.println(b_max); - - return; - // calculate the pole pair number - int pp = round((pp_search_angle)/(angle_end-angle_begin)); - - Serial.print(F("Estimated PP : ")); - Serial.println(pp); - Serial.println(F("PP = Electrical angle / Encoder angle ")); - Serial.print(pp_search_angle*180/M_PI); - Serial.print("/"); - Serial.print((angle_end-angle_begin)*180/M_PI); - Serial.print(" = "); - Serial.println((pp_search_angle)/(angle_end-angle_begin)); - Serial.println(); - - - // a bit of monitoring the result - if(pp <= 0 ){ - Serial.println(F("PP number cannot be negative")); - Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration.")); - return; - }else if(pp > 30){ - Serial.println(F("PP number very high, possible error.")); - }else{ - Serial.println(F("If PP is estimated well your motor should turn now!")); - Serial.println(F(" - If it is not moving try to relaunch the program!")); - Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); - } - - - // set FOC loop to be used - motor.controller = MotionControlType::torque; - // set the pole pair number to the motor - motor.pole_pairs = pp; - //align encoder and start FOC - motor.initFOC(); - _delay(1000); - - Serial.println(F("\n Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); -} - -// uq voltage -float target_voltage = 2; - -void loop() { - - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_voltage); - - // communicate with the user - serialReceiveUserCommand(); -} - - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } -} \ No newline at end of file diff --git a/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino b/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino deleted file mode 100644 index f042661f..00000000 --- a/examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino +++ /dev/null @@ -1,173 +0,0 @@ -/** - * Utility arduino sketch which finds pole pair number of the motor - * - * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin. - * - * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * - * If the code calculates negative pole pair number please invert your motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. - */ -#include - -// BLDC motor instance -// its important to put pole pairs number as 1!!! -BLDCMotor motor = BLDCMotor(1); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor instance -// its important to put pole pairs number as 1!!! -//StepperMotor motor = StepperMotor(1); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); -// magnetic sensor instance - I2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); -// magnetic sensor instance - analog output -// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); - -void setup() { - - // initialise magnetic sensor hardware - sensor.init(); - // link the motor to the sensor - motor.linkSensor(&sensor); - - // power supply voltage - // default 12V - driver.voltage_power_supply = 12; - driver.init(); - motor.linkDriver(&driver); - - // initialize motor hardware - motor.init(); - - // monitoring port - Serial.begin(115200); - - // pole pairs calculation routine - Serial.println("Pole pairs (PP) estimator"); - Serial.println("-\n"); - - float pp_search_voltage = 4; // maximum power_supply_voltage/2 - float pp_search_angle = 6*M_PI; // search electrical angle to turn - - // move motor to the electrical angle 0 - motor.controller = MotionControlType::angle_openloop; - motor.voltage_limit=pp_search_voltage; - motor.move(0); - _delay(1000); - // read the sensor angle - sensor.update(); - float angle_begin = sensor.getAngle(); - _delay(50); - - // move the motor slowly to the electrical angle pp_search_angle - float motor_angle = 0; - while(motor_angle <= pp_search_angle){ - motor_angle += 0.01f; - sensor.update(); // keep track of the overflow - motor.move(motor_angle); - _delay(1); - } - _delay(1000); - // read the sensor value for 180 - sensor.update(); - float angle_end = sensor.getAngle(); - _delay(50); - // turn off the motor - motor.move(0); - _delay(1000); - - // calculate the pole pair number - int pp = round((pp_search_angle)/(angle_end-angle_begin)); - - Serial.print(F("Estimated PP : ")); - Serial.println(pp); - Serial.println(F("PP = Electrical angle / Encoder angle ")); - Serial.print(pp_search_angle*180/M_PI); - Serial.print(F("/")); - Serial.print((angle_end-angle_begin)*180/M_PI); - Serial.print(F(" = ")); - Serial.println((pp_search_angle)/(angle_end-angle_begin)); - Serial.println(); - - - // a bit of monitoring the result - if(pp <= 0 ){ - Serial.println(F("PP number cannot be negative")); - Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration.")); - return; - }else if(pp > 30){ - Serial.println(F("PP number very high, possible error.")); - }else{ - Serial.println(F("If PP is estimated well your motor should turn now!")); - Serial.println(F(" - If it is not moving try to relaunch the program!")); - Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); - } - - - // set motion control loop to be used - motor.controller = MotionControlType::torque; - // set the pole pair number to the motor - motor.pole_pairs = pp; - //align sensor and start FOC - motor.initFOC(); - _delay(1000); - - Serial.println(F("\n Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); -} - -// uq voltage -float target_voltage = 2; - -void loop() { - - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_voltage); - - // communicate with the user - serialReceiveUserCommand(); -} - - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } -} From a3c80dad6a1ebc725d1f047cdd7e7bd16eedc4bc Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 3 Feb 2022 11:56:39 +0100 Subject: [PATCH 039/474] Update Commander.h --- src/communication/Commander.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 3e653945..8857c7ab 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -143,7 +143,11 @@ class Commander * 'C' - clear monitor * 'S' - set monitoring variables * 'G' - get variable value - * '' - Target get/set + * '' - Target setting interface + * Depends of the motion control mode: + * - torque : torque (ex. M2.5) + * - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits) + * - angle (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits) * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) From 9451d96d7a27a8afcfbca1326ed0a8de4f6353cd Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 3 Feb 2022 12:25:01 +0100 Subject: [PATCH 040/474] Update Commander.cpp --- src/communication/Commander.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 1d9b4e59..9b4dcccf 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -101,9 +101,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // if target setting if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){ - printVerbose(F("Target: ")); target(motor, user_command); - println(motor->target); return; } @@ -466,7 +464,7 @@ void Commander::scalar(float* value, char* user_cmd){ void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){ // if no values sent if(isSentinel(user_cmd[0])) return; - + float pos, vel, torque; char* next_value; switch(motor->controller){ @@ -546,7 +544,8 @@ void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){ } break; } - //println(*value); + printVerbose(F("Target: ")); + println(motor->target); } From ecd1c25758b3521d58d9a017ec9bf191c77ec4c2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 3 Feb 2022 14:22:32 +0100 Subject: [PATCH 041/474] Update README.md --- README.md | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 105628fd..c6c47b22 100644 --- a/README.md +++ b/README.md @@ -18,21 +18,27 @@ Therefore this is an attempt to: - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) +
      -

      FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

      +

      NEW RELEASE 📢: SimpleFOClibrary v2.2.1 see release

        -
      • Sensor class init bugfix #121
        • -
      • Added the new motion control interface to the commander +
      • Sensor class init bugfix #121
        • +
      • Voltage/current limit handling bugs #118
        • +
      • Added the new motion control interface to the commander see docs
        • New target setting - possible to set the position, velocity and torque target at once
        • Separated the motion control interface from full motor callback - only motion control and torque control type, enable disable and target setting
        -
      • NRF52 series mcus support by @Polyphe
        • -
      • Voltage/current limit handling bugs #118
        • -
      • Generic position and current sense classes - to implement a new sensor only implement one function
        • -
      • Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation
        • -
      • esp32 arduino package transfer to v2.0.1+ - helpful PR#149 by samguns
        • -
      + +
    • New MCU support see docs +
        +
      • NRF52 series mcus support by @Polyphe
        • +
      • esp32 arduino package transfer to v2.0.1+ - helpful PR#149 by samguns
        • +
      • Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation
        • +
      +
      • +
    • Generic sensor class - to implement a new sensor only implement one function see docs
      • +
    ## Arduino *SimpleFOClibrary* v2.2 From 30d131c47ae8202380f782f19c47c1a113d291b4 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Fri, 4 Feb 2022 11:47:31 +0100 Subject: [PATCH 042/474] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 306b4a39..135b250f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -12,7 +12,7 @@ jobs: - arduino:sam:arduino_due_x # arduino due - arduino:samd:nano_33_iot # samd21 - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32 # esp32 + - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo @@ -41,7 +41,7 @@ jobs: platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone - - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino From a1498966f258386799de3123b513b0a5bc3a7e85 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 5 Feb 2022 21:37:35 +0100 Subject: [PATCH 043/474] portenta refactored (but untested) --- .../hardware_specific/portenta_h7_mcu.cpp | 126 +++++++++--------- 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b314ee40..b426f0ed 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -21,13 +21,14 @@ // #define _SOFTWARE_6PWM 0 // #define _ERROR_6PWM -1 -typedef struct{ - int id ; - pwmout_t pins[6]; -} portenta_h7_mcu_enty_s; -portenta_h7_mcu_enty_s motor_slot[10]; -int slot_index = 0; + +typedef struct PortentaDriverParams { + pwmout_t pins[4]; + long pwm_frequency; +// float dead_zone; +} PortentaDriverParams; + /* Convert STM32 Cube HAL channel to LL channel */ @@ -379,112 +380,111 @@ void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +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 - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1])); core_util_critical_section_exit(); - slot_index++; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // 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) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[3]), pinD, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2]), &(params->pins[3])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting //- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ -for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_c); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - for(int i=0; ipins[0]), (float)dc_1a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_1b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_2a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[3]), (float)dc_2b); + core_util_critical_section_exit(); } - +// 6-PWM currently not supported, defer to generic, which also doesn't support it ;-) // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _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){ +//void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ // if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz // else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // // center-aligned frequency is uses two periods @@ -510,13 +510,13 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // _alignPWMTimers(HT1, HT2, HT3); // break; // } - return -1; // success -} +// return -1; // 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, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +//void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ // // find configuration // int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); // // set pwm accordingly @@ -535,5 +535,5 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); // break; // } -} +//} #endif \ No newline at end of file From fba88761892692af551a76ddbe957b0f28e501b2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 5 Feb 2022 22:23:48 +0100 Subject: [PATCH 044/474] STM32 refactored (but untested) --- src/drivers/hardware_specific/stm32_mcu.cpp | 179 ++++++++++++++------ 1 file changed, 128 insertions(+), 51 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 6f03ca80..8f206af4 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -14,15 +14,28 @@ #define _ERROR_6PWM -1 + +typedef struct STM32DriverParams { + HardwareTimer* timers[6]; + uint32_t channels[6]; + long pwm_frequency; + float dead_zone; + uint8_t interface_type; +} STM32DriverParams; + + + + + // setting pwm to hardware pin - instead analogWrite() -void _setPwm(int ulPin, uint32_t value, int resolution) +void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution) { - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + // PinName pin = digitalPinToPinName(ulPin); + // TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + // uint32_t index = get_timer_index(Instance); + // HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + //uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); } @@ -129,7 +142,7 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha } // configure hardware 6pwm interface only one timer with inverted channels -HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +STM32DriverParams* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { #if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface @@ -139,6 +152,12 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in PinName vlPinName = digitalPinToPinName(pinB_l); PinName whPinName = digitalPinToPinName(pinC_h); PinName wlPinName = digitalPinToPinName(pinC_l); + uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)); + uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)); + uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)); + uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)); + uint32_t channel5 = STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)); + uint32_t channel6 = STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); @@ -153,12 +172,12 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(channel3, TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(channel4, TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(channel5, TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(channel6, TIMER_OUTPUT_COMPARE_PWM1, wlPinName); // dead time is set in nanoseconds uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; @@ -177,9 +196,18 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->getHandle()->Instance->CNT = TIM1->ARR; HT->resume(); - return HT; + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT }, + .channels = { channel1, channel3, channel5 }, + .pwm_frequency = PWM_freq, + .dead_zone = dead_zone, + .interface_type = _HARDWARE_6PWM + }; + + return params; #else - return nullptr; // return nothing + return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing #endif } @@ -223,7 +251,7 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +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 // center-aligned frequency is uses two periods @@ -233,13 +261,24 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); // allign the timers _alignPWMTimers(HT1, HT2, HT2); + + uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); + uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2 }, + .channels = { channel1, channel2 }, + .pwm_frequency = pwm_frequency + }; + return params; } + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods @@ -250,12 +289,25 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); // allign the timers _alignPWMTimers(HT1, HT2, HT3); + + uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); + uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); + uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC), PinMap_PWM)); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2, HT3 }, + .channels = { channel1, channel2, channel3 }, + .pwm_frequency = pwm_frequency + }; + return params; } + + // 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) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods @@ -267,37 +319,51 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); + + uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); + uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); + uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC), PinMap_PWM)); + uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinD), PinMap_PWM)); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2, HT3, HT4 }, + .channels = { channel1, channel2, channel3, channel4 }, + .pwm_frequency = pwm_frequency + }; + return params; } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting //- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); - _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); - _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); - _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); + _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); } @@ -306,7 +372,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _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){ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // center-aligned frequency is uses two periods @@ -314,47 +380,58 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // find configuration int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + STM32DriverParams* params; // configure accordingly switch(config){ case _ERROR_6PWM: - return -1; // fail - break; + return SIMPLEFOC_DRIVER_INIT_FAILED; case _HARDWARE_6PWM: - _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); break; case _SOFTWARE_6PWM: HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); _initPinPWMLow(pwm_frequency, pinA_l); - HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency, pinB_h); _initPinPWMLow(pwm_frequency, pinB_l); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinC_h); _initPinPWMLow(pwm_frequency, pinC_l); _alignPWMTimers(HT1, HT2, HT3); + uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA_h), PinMap_PWM)); + uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA_l), PinMap_PWM)); + uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB_h), PinMap_PWM)); + uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB_l), PinMap_PWM)); + uint32_t channel5 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC_h), PinMap_PWM)); + uint32_t channel6 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC_l), PinMap_PWM)); + params = new STM32DriverParams { + .timers = { HT1, HT2, HT3 }, + .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone, + .interface_type = _SOFTWARE_6PWM + }; break; } - return 0; // success + 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, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // find configuration - int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - // set pwm accordingly - switch(config){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: - _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + float dead_zone = ((STM32DriverParams*)params)->dead_zone; + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); break; } } From 514f1c1080072c707f76b845a83ceb50a0af9134 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 5 Feb 2022 22:35:34 +0100 Subject: [PATCH 045/474] STM32 driver refactor (not yet tested) --- src/drivers/hardware_specific/stm32_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 8f206af4..a20cbbc8 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -142,7 +142,7 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha } // configure hardware 6pwm interface only one timer with inverted channels -STM32DriverParams* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { #if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface @@ -168,7 +168,7 @@ STM32DriverParams* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); From cc3393d8d2adf4ac2f19bde30f16505dc0c57043 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 00:31:07 +0100 Subject: [PATCH 046/474] esp32 driver refactor (untested) --- src/drivers/hardware_specific/esp32_mcu.cpp | 143 ++++++++++++-------- 1 file changed, 88 insertions(+), 55 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 7933e13a..6cba5768 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -35,6 +35,7 @@ typedef struct { mcpwm_io_signals_t mcpwm_b; mcpwm_io_signals_t mcpwm_c; } bldc_3pwm_motor_slots_t; + typedef struct { int pin1A; mcpwm_dev_t* mcpwm_num; @@ -46,6 +47,7 @@ typedef struct { mcpwm_io_signals_t mcpwm_2a; mcpwm_io_signals_t mcpwm_2b; } stepper_4pwm_motor_slots_t; + typedef struct { int pin1pwm; mcpwm_dev_t* mcpwm_num; @@ -69,6 +71,8 @@ typedef struct { mcpwm_io_signals_t mcpwm_cl; } bldc_6pwm_motor_slots_t; + + // define bldc motor slots array bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A @@ -97,6 +101,19 @@ stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B }; + + +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; +} ESP32MCPWMDriverParams; + + + + + // configuring high frequency pwm timer // a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller @@ -180,7 +197,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max @@ -218,6 +235,12 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator + }; + return params; } @@ -225,7 +248,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - BLDC motor - 3PWM setting // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max @@ -263,13 +286,19 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator + }; + return params; } // 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) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; @@ -312,66 +341,68 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator1, + .mcpwm_operator2 = m_slot.mcpwm_operator2 + }; + return params; } + + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - break; - } - } +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); - break; - } - } +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); } + + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); - break; - } - } +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); } + + + // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _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){ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency @@ -387,7 +418,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } } // if no slots available - if(slot_num >= 2) return -1; + if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; // disable all the slots with the same MCPWM if( slot_num == 0 ){ @@ -415,26 +446,28 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); // return - return 0; + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_unit = m_slot.mcpwm_unit + }; + return params; } + + + // 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, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - break; - } - } + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); } + #endif From d243cbf05ffa77246152b1b30e2375fa1eded7af Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 01:05:48 +0100 Subject: [PATCH 047/474] made ESP32 LEDC driver selectable via build flag --- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 6cba5768..6f6b5018 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" From e433f957377aad543a0091886dc81273e5f82b93 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 01:06:40 +0100 Subject: [PATCH 048/474] refactor and simplify LEDC driver (untested) --- .../hardware_specific/esp32_ledc_mcu.cpp | 222 +++++++----------- 1 file changed, 83 insertions(+), 139 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index ea11c6d7..55348e2b 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) #include "driver/ledc.h" @@ -25,6 +25,7 @@ #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif + // current channel stack index // support for multiple motors // esp32 has 16 channels @@ -32,50 +33,17 @@ // esp32c3 has 6 channels int channel_index = 0; -// slot for the 3pwm bldc motors -typedef struct { - int pinID; - int ch1; - int ch2; - int ch3; -} bldc_3pwm_motor_slots_t; - -// slot for the 2pwm stepper motors -typedef struct { - int pinID; - int ch1; - int ch2; -} stepper_2pwm_motor_slots_t; - -// slot for the 4pwm stepper motors -typedef struct { - int pinID; - int ch1; - int ch2; - int ch3; - int ch4; -} stepper_4pwm_motor_slots_t; - - -// define motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { - {_EMPTY_SLOT, 0,0,0}, // 1st motor - {_EMPTY_SLOT, 0,0,0}, // 2nd motor - {_EMPTY_SLOT, 0,0,0}, // 3st motor // esp32s2 & esp32 - {_EMPTY_SLOT, 0,0,0}, // 4nd motor // esp32 only -}; -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4]={ - {_EMPTY_SLOT, 0,0}, // 1st motor - {_EMPTY_SLOT, 0,0}, // 2nd motor - {_EMPTY_SLOT, 0,0}, // 3rd motor - {_EMPTY_SLOT, 0,0} // 4th motor - esp32s2 and esp32 -}; -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[4]={ - {_EMPTY_SLOT, 0,0,0,0}, // 1st motor - {_EMPTY_SLOT, 0,0,0,0}, // 2nd motor - esp32s2 and esp32 - {_EMPTY_SLOT, 0,0,0,0}, // 3st motor - only esp32 - {_EMPTY_SLOT, 0,0,0,0}, // 4st motor - only esp32 -}; + + + +typedef struct ESP32LEDCDriverParams { + int channels[6]; + long pwm_frequency; +} ESP32LEDCDriverParams; + + + + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin, const int channel){ @@ -83,122 +51,98 @@ void _setHighFrequency(const long freq, const int pin, const int channel){ ledcAttachPin(pin, channel); } -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + + +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 2 >= LEDC_CHANNELS ) return; - - stepper_2pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - for(int slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_2pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_2pwm_motor_slots[slot_num].pinID = pinA; - esp32_stepper_2pwm_motor_slots[slot_num].ch1 = channel_index++; - esp32_stepper_2pwm_motor_slots[slot_num].ch2 = channel_index++; - m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; - break; - } - } - - _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); - _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); + if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2 }, + .pwm_frequency = pwm_frequency + }; + return params; } -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 3 >= LEDC_CHANNELS ) return; - - bldc_3pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - for(int slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_bldc_3pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_3pwm_motor_slots[slot_num].pinID = pinA; - esp32_bldc_3pwm_motor_slots[slot_num].ch1 = channel_index++; - esp32_bldc_3pwm_motor_slots[slot_num].ch2 = channel_index++; - esp32_bldc_3pwm_motor_slots[slot_num].ch3 = channel_index++; - m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; - break; - } - } - - _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); - _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); - _setHighFrequency(pwm_frequency, pinC, m_slot.ch3); + if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + int ch3 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + _setHighFrequency(pwm_frequency, pinC, ch3); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2, ch3 }, + .pwm_frequency = pwm_frequency + }; + return params; } -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 4 >= LEDC_CHANNELS ) return; - - - stepper_4pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - for(int slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_4pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_4pwm_motor_slots[slot_num].pinID = pinA; - esp32_stepper_4pwm_motor_slots[slot_num].ch1 = channel_index++; - esp32_stepper_4pwm_motor_slots[slot_num].ch2 = channel_index++; - esp32_stepper_4pwm_motor_slots[slot_num].ch3 = channel_index++; - esp32_stepper_4pwm_motor_slots[slot_num].ch4 = channel_index++; - m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; - break; - } - } - - _setHighFrequency(pwm_frequency, pinA, m_slot.ch1); - _setHighFrequency(pwm_frequency, pinB, m_slot.ch2); - _setHighFrequency(pwm_frequency, pinC, m_slot.ch3); - _setHighFrequency(pwm_frequency, pinD, m_slot.ch4); + if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + int ch3 = channel_index++; + int ch4 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + _setHighFrequency(pwm_frequency, pinC, ch3); + _setHighFrequency(pwm_frequency, pinD, ch4); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2, ch3, ch4 }, + .pwm_frequency = pwm_frequency + }; + return params; } -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ -// determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_stepper_2pwm_motor_slots[i].pinID == pinA){ // if motor slot found - ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - break; - } - } + + + +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)); } -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_bldc_3pwm_motor_slots[i].pinID == pinA){ // if motor slot found - ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); - break; - } - } + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); } -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_stepper_4pwm_motor_slots[i].pinID == pin1A){ // if motor slot found - ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch4, _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); - break; - } - } + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); } #endif \ No newline at end of file From b9257bce31d57a45671317a5d491bc45b8cb935a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 17:13:45 +0100 Subject: [PATCH 049/474] nrf52 driver refactored (but untested) --- src/drivers/hardware_specific/nrf52_mcu.cpp | 138 ++++++++++++-------- 1 file changed, 82 insertions(+), 56 deletions(-) diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index 89e58e6c..f74eafc5 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -22,7 +22,7 @@ #define _TAKEN_SLOT (-0x55) int pwm_range; -float dead_time; + static NRF_PWM_Type* pwms[PWM_COUNT] = { NRF_PWM0, @@ -72,7 +72,22 @@ stepper_motor_slots_t nrf52_stepper_motor_slots[4] = { bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1 {_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2 - }; + }; + + + +typedef struct NRF52DriverParams { + union { + bldc_3pwm_motor_slots_t* slot3pwm; + bldc_6pwm_motor_slots_t* slot6pwm; + stepper_motor_slots_t* slotstep; + } slot; + long pwm_frequency; + float dead_time; +} NRF52DriverParams; + + + // configuring high frequency pwm timer void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){ @@ -103,7 +118,7 @@ void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){ // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 @@ -123,6 +138,9 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } + // if no slots available + if(slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED; + // disable all the slots with the same MCPWM if(slot_num < 2){ // slot 0 of the stepper @@ -149,12 +167,20 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the pwm _configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm); + + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slot3pwm = &(nrf52_bldc_3pwm_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + return params; } + + + // 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) { +void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 @@ -175,8 +201,11 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } + // if no slots available + if (slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED; + // disable all the slots with the same MCPWM - if( slot_num < 2 ){ + if (slot_num < 2){ // slots 0 and 1 of the 3pwm bldc nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; // slot 0 of the 6pwm bldc @@ -202,54 +231,51 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the pwm _configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm); + + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slotstep = &(nrf52_stepper_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + return params; } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(nrf52_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,range] - - nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000; - nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000; - nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000; - - nrf52_bldc_3pwm_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1; - break; - } - } +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + // transform duty cycle from [0,1] to [0,range] + bldc_3pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot3pwm; + p->mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000; + + p->mcpwm->TASKS_SEQSTART[0] = 1; } + + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(nrf52_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,range] - - nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000; - nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000; - nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000; - nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000; - - nrf52_stepper_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1; - break; - } - } +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + + stepper_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slotstep; + p->mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000; + + p->mcpwm->TASKS_SEQSTART[0] = 1; } /* Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific */ -int _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){ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400 else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256 @@ -257,6 +283,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const pwm_range = (PWM_CLK / pwm_frequency); pwm_range /= 2; // scale the frequency (centered PWM) + float dead_time; if (dead_zone != NOT_SET){ dead_time = dead_zone/2; }else{ @@ -281,7 +308,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } } // if no slots available - if(slot_num >= 2) return -1; + if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; // disable all the slots with the same MCPWM if( slot_num == 0 ){ @@ -324,31 +351,30 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the pwm type _configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2); - // return - return 0; + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slot6pwm = &(nrf52_bldc_6pwm_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + params->dead_time = dead_time; + return params; } + + + /* Function setting the duty cycle to the pwm pin // - BLDC driver - 6PWM setting // - hardware specific */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, const int pinA_h, const int, const int, const int, const int, const int){ - for(int i = 0; i < 2; i++){ - if(nrf52_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,range] - - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000; - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range); - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000; - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range); - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000; - nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range); - +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + bldc_6pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot6pwm; + float dead_time = ((NRF52DriverParams*)params)->dead_time; + p->mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range); + p->mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range); + p->mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range); NRF_EGU0->TASKS_TRIGGER[0] = 1; - break; - } - } } From a2344ef81d6fb74b3ce690054c2c5766b5c21164 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 17:46:50 +0100 Subject: [PATCH 050/474] make 2-pwm unsupported explicit --- src/drivers/hardware_specific/nrf52_mcu.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index f74eafc5..2dcadc4f 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -115,6 +115,16 @@ void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){ } } + + +// can we support it using the generic driver on this MCU? Commented out to fall back to generic driver for 2-pwm +// void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +// return SIMPLEFOC_DRIVER_INIT_FAILED; // not supported +// } + + + + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic From b2773d8cdd68bc786f8385016e75aae8cbd5c5a9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Feb 2022 17:54:26 +0100 Subject: [PATCH 051/474] due driver refactor (but untested) --- src/drivers/hardware_specific/due_mcu.cpp | 67 ++++++++++++++++++----- 1 file changed, 52 insertions(+), 15 deletions(-) diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index e0d39afd..80ef82d9 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -320,13 +320,17 @@ void TC8_Handler() if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]); } + + + + // implementation of the hardware_api.cpp // --------------------------------------------------------------------------------------------------------------------------------- // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware specific -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -337,13 +341,21 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinC, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } + + // Configuring PWM frequency, resolution and alignment //- Stepper driver - 2PWM setting // - hardware specific -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -353,12 +365,21 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { initPWM(pinB, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // 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) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -370,36 +391,52 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinD, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC, pinD); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); - setPwm(pinC, _max_pwm_value*dc_c); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); + setPwm(p->pins[2], _max_pwm_value*dc_c); } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pin1A, _max_pwm_value*dc_1a); - setPwm(pin1B, _max_pwm_value*dc_1b); - setPwm(pin2A, _max_pwm_value*dc_2a); - setPwm(pin2B, _max_pwm_value*dc_2b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_1a); + setPwm(p->pins[1], _max_pwm_value*dc_1b); + setPwm(p->pins[2], _max_pwm_value*dc_2a); + setPwm(p->pins[3], _max_pwm_value*dc_2b); } + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - Stepper driver - 2PWM setting // - hardware specific -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); } From 8db941aa84ea19ac33a9cd139363774b62d76374 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Feb 2022 11:42:49 +0100 Subject: [PATCH 052/474] fixup SAMD driver refactor (working, tested) --- src/drivers/hardware_specific/samd_mcu.cpp | 27 +++++++++++----------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 02f27d9d..b950fd09 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -682,7 +682,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { * @param pinB phase B hardware pin number * @param pinC phase C hardware pin number */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC, void* params) { +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a); writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b); writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_c); @@ -706,7 +706,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB * @param pin2A phase 2A hardware pin number * @param pin2B phase 2B hardware pin number */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B, void* params){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_1a); writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_1b); writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_2a); @@ -740,13 +740,14 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in * @param pinC_l phase C low-side hardware pin number * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l, float dead_zone, void* params){ - tccConfiguration* tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0]; - tccConfiguration* tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1]; - uint32_t pwm_res =((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0]->pwm_res; +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + SAMDHardwareDriverParams* p = (SAMDHardwareDriverParams*)params; + tccConfiguration* tcc1 = p->tccPinConfigurations[0]; + tccConfiguration* tcc2 = p->tccPinConfigurations[1]; + uint32_t pwm_res =p->tccPinConfigurations[0]->pwm_res; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... - float ls = dc_a+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); // TODO resolution!!! + float ls = dc_a+(p->dead_zone * (pwm_res-1)); // TODO resolution!!! if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_a); writeSAMDDutyCycle(tcc2, ls); @@ -754,10 +755,10 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, int pinA_h, int pi else writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly - tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2]; - tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[3]; + tcc1 = p->tccPinConfigurations[2]; + tcc2 = p->tccPinConfigurations[3]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_b+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); + float ls = dc_b+(p->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_b); writeSAMDDutyCycle(tcc2, ls); @@ -765,10 +766,10 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, int pinA_h, int pi else writeSAMDDutyCycle(tcc1, dc_b); - tcc1 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[4]; - tcc2 = ((SAMDHardwareDriverParams*)params)->tccPinConfigurations[5]; + tcc1 = p->tccPinConfigurations[4]; + tcc2 = p->tccPinConfigurations[5]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_c+(((SAMDHardwareDriverParams*)params)->dead_zone * (pwm_res-1)); + float ls = dc_c+(p->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1, dc_c); writeSAMDDutyCycle(tcc2, ls); From 5889c49ba290ab3d5add5d558297c39c6ba5b7d0 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 12 Feb 2022 09:10:46 +0100 Subject: [PATCH 053/474] some comments and small typo for atmega328 --- src/drivers/hardware_api.h | 5 +++++ src/drivers/hardware_specific/atmega328_mcu.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 84eb5999..01f251b9 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -4,8 +4,13 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +// flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) +// generic implementation of the hardware specific structure +// containing all the necessary driver parameters +// 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]; long pwm_frequency; diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 8bf58835..33e4d497 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -72,7 +72,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 +// supports Arduino/ATmega328 void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz @@ -128,7 +128,7 @@ int _configureComplementaryPair(int pinH, int pinL) { } // Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting +// - BLDC driver - setting // - hardware specific // supports Arudino/ATmega328 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) { @@ -140,8 +140,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons ret_flag += _configureComplementaryPair(pinC_h, pinC_l); if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .pwm_frequency = pwm_frequency + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, .dead_zone = dead_zone }; return params; From aefee3f679114acd2d42e320101a5ad6c784f1e3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 13:29:54 +0100 Subject: [PATCH 054/474] initial debug commit for merge --- src/BLDCMotor.cpp | 50 ++++++++++---------- src/StepperMotor.cpp | 38 +++++++-------- src/communication/SimpleFOCDebug.cpp | 56 ++++++++++++++++++++++ src/communication/SimpleFOCDebug.h | 71 ++++++++++++++++++++++++++++ 4 files changed, 169 insertions(+), 46 deletions(-) create mode 100644 src/communication/SimpleFOCDebug.cpp create mode 100644 src/communication/SimpleFOCDebug.h diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d47ca51f..8f090fb1 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -24,7 +24,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); + SIMPLEFOC_DEBUG("MOT: Init"); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && _isset(phase_resistance)) { @@ -54,7 +54,7 @@ void BLDCMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); } @@ -104,7 +104,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else + SIMPLEFOC_DEBUG("MOT: No sensor."); // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases @@ -112,13 +113,13 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction _delay(500); if(exit_flag){ if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); + else SIMPLEFOC_DEBUG("MOT: No current sense."); } if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); disable(); } @@ -129,18 +130,17 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction int BLDCMotor::alignCurrentSense() { int exit_flag = 1; // success - if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); + SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); if(!exit_flag){ // error in current sense - phase either not measured or bad connection - if(monitor_port) monitor_port->println(F("MOT: Align error!")); + SIMPLEFOC_DEBUG("MOT: Align error!"); exit_flag = 0; }else{ // output the alignment status flag - if(monitor_port) monitor_port->print(F("MOT: Success: ")); - if(monitor_port) monitor_port->println(exit_flag); + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); } return exit_flag > 0; @@ -149,7 +149,7 @@ int BLDCMotor::alignCurrentSense() { // Encoder alignment to electrical 0 angle int BLDCMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction if(!_isset(sensor_direction)){ @@ -180,24 +180,23 @@ int BLDCMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -213,13 +212,12 @@ int BLDCMotor::alignSensor() { //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); return exit_flag; } @@ -228,7 +226,7 @@ int BLDCMotor::alignSensor() { int BLDCMotor::absoluteZeroSearch() { // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision // of float is sufficient. - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -248,8 +246,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else SIMPLEFOC_DEBUG("MOT: Success!"); } return !sensor->needsSearch(); } @@ -298,7 +296,7 @@ void BLDCMotor::loopFOC() { break; default: // no torque control selected - if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); break; } diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2acd6f11..2f30f373 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -25,7 +25,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { // init hardware pins void StepperMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); + SIMPLEFOC_DEBUG("MOT: Init"); // if set the phase resistance of the motor use current limit to calculate the voltage limit if(_isset(phase_resistance)) { @@ -49,7 +49,7 @@ void StepperMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); @@ -101,12 +101,12 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else SIMPLEFOC_DEBUG("MOT: No sensor."); if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); disable(); } @@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction if(!_isset(sensor_direction)){ @@ -147,24 +147,23 @@ int StepperMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -179,13 +178,12 @@ int StepperMotor::alignSensor() { zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); return exit_flag; } @@ -193,7 +191,7 @@ int StepperMotor::alignSensor() { // - to the index int StepperMotor::absoluteZeroSearch() { - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -213,8 +211,8 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else SIMPLEFOC_DEBUG("MOT: Success!"); } return !sensor->needsSearch(); } diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp new file mode 100644 index 00000000..871fefc6 --- /dev/null +++ b/src/communication/SimpleFOCDebug.cpp @@ -0,0 +1,56 @@ + +#include "SimpleFOCDebug.h" + +#ifndef SIMPLEFOC_DISABLE_DEBUG + + +Print* SimpleFOCDebug::_debugPrint = NULL; + + +void SimpleFOCDebug::enable(Print* debugPrint) { + _debugPrint = debugPrint; +} + + + +void SimpleFOCDebug::println(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + +void SimpleFOCDebug::println(const char* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const char* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +#endif \ No newline at end of file diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h new file mode 100644 index 00000000..bc8a61b2 --- /dev/null +++ b/src/communication/SimpleFOCDebug.h @@ -0,0 +1,71 @@ + +#ifndef __SIMPLEFOCDEBUG_H__ +#define __SIMPLEFOCDEBUG_H__ + +#include "Arduino.h" + + +/** + * SimpleFOCDebug class + * + * This class is used to print debug messages to a chosen output. + * Currently, Print instances are supported as targets, e.g. serial port. + * + * Activate debug output globally by calling enable(), optionally passing + * in a Print instance. If none is provided "Serial" is used by default. + * + * To produce debug output, use the macro SIMPLEFOC_DEBUG: + * SIMPLEFOC_DEBUG("Debug message!"); + * SIMPLEFOC_DEBUG("a float value:", 123.456); + * SIMPLEFOC_DEBUG("an integer value: ", 123); + * + * Keep debugging output short and simple. Some of our MCUs have limited + * RAM and limited serial output capabilities. + * + * By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to + * help preserve memory on Arduino boards. + * + * You can also disable debug output completely. In this case all debug output + * and the SimpleFOCDebug class is removed from the compiled code. + * Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in + * this way. + * + **/ + + +#ifndef SIMPLEFOC_DISABLE_DEBUG + +class SimpleFOCDebug { +public: + void enable(Print* debugPrint = Serial); + + void println(const __FlashStringHelper* msg); + void println(const char* msg); + void println(const __FlashStringHelper* msg, float val); + void println(const char* msg, float val); + void println(const __FlashStringHelper* msg, int val); + void println(const char* msg, int val); + +protected: + static Print* _debugPrint; +}; + + +#define SIMPLEFOC_DEBUG(msg, ...) \ + SimpleFOCDebug::println(F(msg) __VA_OPT__(,) __VA_ARGS__) + + + + + +#else //ifndef SIMPLEFOC_DISABLE_DEBUG + + + +#define SIMPLEFOC_DEBUG(msg, ...) + + + +#endif //ifndef SIMPLEFOC_DISABLE_DEBUG +#endif + From 30510c7d553fb44cbf56d30ca8a033a0dccf567b Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 12 Feb 2022 15:40:07 +0100 Subject: [PATCH 055/474] intermediate commit for reaftoring of current sense --- .../B_G431B_ESC1/B_G431B_ESC1.ino | 2 + .../esp32_current_control_low_side.ino | 5 +- .../single_full_control_example.ino | 2 + .../double_full_control_example.ino | 5 + .../single_full_control_example.ino | 2 + .../current_control/current_control.ino | 2 + keywords.txt | 2 +- src/BLDCMotor.cpp | 2 +- src/common/base_classes/CurrentSense.cpp | 7 + src/common/base_classes/CurrentSense.h | 23 ++-- src/current_sense/GenericCurrentSense.cpp | 12 +- src/current_sense/GenericCurrentSense.h | 5 +- src/current_sense/InlineCurrentSense.cpp | 29 ++--- src/current_sense/InlineCurrentSense.h | 5 +- src/current_sense/LowsideCurrentSense.cpp | 30 ++--- src/current_sense/LowsideCurrentSense.h | 5 +- src/current_sense/hardware_api.h | 29 ++++- .../hardware_specific/atmega_mcu.cpp | 37 ++---- .../hardware_specific/due_mcu.cpp | 29 ++--- .../hardware_specific/esp32_ledc_mcu.cpp | 27 ++-- .../hardware_specific/esp32_mcu.cpp | 122 ++++++++++++++---- .../hardware_specific/generic_mcu.cpp | 30 +++-- .../hardware_specific/stm32_mcu.cpp | 30 ++--- .../hardware_specific/stm32g4_mcu.cpp | 31 +++-- .../hardware_specific/teensy_mcu.cpp | 29 ++--- .../hardware_specific/esp32_driver_mcpwm.h | 88 +++++++++++++ src/drivers/hardware_specific/esp32_mcu.cpp | 91 +------------ 27 files changed, 374 insertions(+), 307 deletions(-) create mode 100644 src/drivers/hardware_specific/esp32_driver_mcpwm.h diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 83451aa7..828d853d 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -40,6 +40,8 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); + // link current sense and the driver + currentSense.linkDriver(&driver); // current sensing currentSense.init(); diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 202aa337..84033b42 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -79,6 +79,10 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage motor.voltage_sensor_align = 0.5; // control loop type and torque mode @@ -124,7 +128,6 @@ void setup() { motor.init(); cs.init(); - cs.driverSync(&driver); // driver 8302 has inverted gains on all channels cs.gain_a *=-1; cs.gain_b *=-1; diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index dfad6e3a..a508bb19 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -33,6 +33,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); motor.voltage_sensor_align = 1; // set control loop type to be used diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 259b23ee..2d28790c 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -56,11 +56,16 @@ void setup() { driver1.init(); // link driver motor1.linkDriver(&driver1); + // link current sense and the driver + current_sense1.linkDriver(&driver1); + // power supply voltage [V] driver2.voltage_power_supply = 12; driver2.init(); // link driver motor2.linkDriver(&driver2); + // link current sense and the driver + current_sense2.linkDriver(&driver2); // set control loop type to be used motor1.controller = MotionControlType::torque; diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index c1b224e6..688c8b6a 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -34,6 +34,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // set control loop type to be used motor.controller = MotionControlType::torque; diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index 8b1ec96b..3f3ccb18 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -39,6 +39,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // current sense init hardware current_sense.init(); diff --git a/keywords.txt b/keywords.txt index add21dce..6a2c2af6 100644 --- a/keywords.txt +++ b/keywords.txt @@ -91,7 +91,7 @@ getFOCCurrents KEYWORD2 getDCCurrent KEYWORD2 setPwm KEYWORD2 driverAlign KEYWORD2 -driverSync KEYWORD2 +linkDriver KEYWORD2 add KEYWORD2 run KEYWORD2 attach KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 6b46572d..9c62ff2d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -143,7 +143,7 @@ int BLDCMotor::alignCurrentSense() { if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); // align current sense and the driver - exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align); if(!exit_flag){ // error in current sense - phase either not measured or bad connection if(monitor_port) monitor_port->println(F("MOT: Align error!")); diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 2cec8dd5..dd3ab524 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -64,4 +64,11 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ return_current.d = i_alpha * ct + i_beta * st; return_current.q = i_beta * ct - i_alpha * st; return return_current; +} + +/** + Driver linking to the current sense +*/ +void CurrentSense::linkDriver(BLDCDriver* _driver) { + driver = _driver; } \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 0904dea2..ad1bd9ca 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -13,19 +13,24 @@ class CurrentSense{ /** * Function intialising the CurrentSense class - * All the necessary intialisations of adc and sync should be implemented here + * - All the necessary intialisations of adc and sync should be implemented here + * + * @returns - 0 - for failure & 1 - for success */ - virtual void init() = 0; + virtual int init() = 0; /** - * Function intended to implement all that is needed to sync and current sensing with the driver. - * If no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & 1 - for success + * Linking the current sense with the motor driver + * Only necessary if synchronisation in between the two is required */ - virtual int driverSync(BLDCDriver *driver) = 0; + void linkDriver(BLDCDriver *driver); - // calibration variables + // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() + + BLDCDriver* driver; //!< driver link + void* params = 0; //!< pointer to hardware specific parameters of current sensing + /** * Function intended to verify if: * - phase current are oriented properly @@ -34,7 +39,7 @@ class CurrentSense{ * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) * @returns - 0 - for failure & positive number (with status) - for success */ - virtual int driverAlign(BLDCDriver *driver, float voltage) = 0; + virtual int driverAlign(float align_voltage) = 0; /** * Function rading the phase currents a, b and c @@ -62,6 +67,8 @@ class CurrentSense{ * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); + + }; #endif \ No newline at end of file diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index a72d5232..bd1a4ddc 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -8,11 +8,13 @@ GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void } // Inline sensor init function -void GenericCurrentSense::init(){ +int GenericCurrentSense::init(){ // configure ADC variables if(initCallback != nullptr) initCallback(); // calibrate zero offsets calibrateOffsets(); + // return success + return 1; } // Function finding zero offsets of the ADC void GenericCurrentSense::calibrateOffsets(){ @@ -45,12 +47,6 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ return current; } -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int GenericCurrentSense::driverSync(BLDCDriver *driver){ - return 1; -} - // Function aligning the current sense with motor driver // if all pins are connected well none of this is really necessary! - can be avoided // returns flag @@ -59,7 +55,7 @@ int GenericCurrentSense::driverSync(BLDCDriver *driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ +int GenericCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index 2f24e134..a63df49d 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -18,10 +18,9 @@ class GenericCurrentSense: public CurrentSense{ GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; + int driverAlign(float align_voltage) override; PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index bc3bc4c6..90a8333c 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -20,25 +20,29 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ } // Inline sensor init function -void InlineCurrentSense::init(){ +int InlineCurrentSense::init(){ // configure ADC variables - _configureADCInline(pinA,pinB,pinC); + params = _configureADCInline(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); + // return success + return 1; } // Function finding zero offsets of the ADC void InlineCurrentSense::calibrateOffsets(){ const int calibration_rounds = 1000; - + // find adc offset = zero current voltage offset_ia = 0; offset_ib = 0; offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltageInline(pinA); - offset_ib += _readADCVoltageInline(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); + offset_ia += _readADCVoltageInline(pinA, params); + offset_ib += _readADCVoltageInline(pinB, params); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params); _delay(1); } // calculate the mean offsets @@ -50,16 +54,11 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int InlineCurrentSense::driverSync(BLDCDriver *driver){ - return 1; -} // Function aligning the current sense with motor driver // if all pins are connected well none of this is really necessary! - can be avoided @@ -69,7 +68,7 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ +int InlineCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 748d3f44..9ee25740 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -23,10 +23,9 @@ class InlineCurrentSense: public CurrentSense{ InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; + int driverAlign(float align_voltage) override; // ADC measuremnet gain for each phase // support for different gains for different phases of more commonly - inverted phase currents diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index ca4f51b4..4c747b5f 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -20,13 +20,17 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int } // Lowside sensor init function -void LowsideCurrentSense::init(){ +int LowsideCurrentSense::init(){ // configure ADC variables - _configureADCLowSide(pinA,pinB,pinC); + params = _configureADCLowSide(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(); + _driverSyncLowSide(driver->params, params); // calibrate zero offsets calibrateOffsets(); + // return success + return 1; } // Function finding zero offsets of the ADC void LowsideCurrentSense::calibrateOffsets(){ @@ -39,9 +43,9 @@ void LowsideCurrentSense::calibrateOffsets(){ // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { _startADC3PinConversionLowSide(); - offset_ia += _readADCVoltageLowSide(pinA); - offset_ib += _readADCVoltageLowSide(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + offset_ia += _readADCVoltageLowSide(pinA, params); + offset_ib += _readADCVoltageLowSide(pinB, params); + if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC, params); _delay(1); } // calculate the mean offsets @@ -54,17 +58,11 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int LowsideCurrentSense::driverSync(BLDCDriver *driver){ - _driverSyncLowSide(); - return 1; -} // Function aligning the current sense with motor driver // if all pins are connected well none of this is really necessary! - can be avoided @@ -74,7 +72,7 @@ int LowsideCurrentSense::driverSync(BLDCDriver *driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ +int LowsideCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 057aa623..0eff3abe 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -24,10 +24,9 @@ class LowsideCurrentSense: public CurrentSense{ LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; + int driverAlign(float align_voltage) override; // ADC measuremnet gain for each phase // support for different gains for different phases of more commonly - inverted phase currents diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index e1b50003..7862b708 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -4,30 +4,46 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +// flag returned if current sense init fails +#define SIMPLEFOC_CURRENT_SENSE_INIT_FAILED ((void*)-1) + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct GenericCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; +} GenericCurrentSenseParams; + + /** * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageInline(const int pinA); +float _readADCVoltageInline(const int pinA, const void* cs_params); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCInline(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCLowSide(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); void _startADC3PinConversionLowSide(); @@ -35,12 +51,15 @@ void _startADC3PinConversionLowSide(); * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageLowSide(const int pinA); +float _readADCVoltageLowSide(const int pinA, const void* cs_params); /** * function syncing the Driver with the ADC for the LowSide Sensing + * @param driver_params - driver parameter structure - hardware specific + * @param cs_params - current sense parameter structure - hardware specific */ -void _driverSyncLowSide(); +void _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 75265c03..c70e18a5 100644 --- a/src/current_sense/hardware_specific/atmega_mcu.cpp +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -5,10 +5,6 @@ #define _ADC_VOLTAGE 5.0f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - #ifndef cbi #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #endif @@ -16,40 +12,29 @@ #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) #endif -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample cbi(ADCSRA, ADPS2); sbi(ADCSRA, ADPS1); sbi(ADCSRA, ADPS0); -} - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + return params; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp index ef450b35..ffeae581 100644 --- a/src/current_sense/hardware_specific/due_mcu.cpp +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -5,34 +5,23 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); -} + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + return params; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp index 79d335e9..c0e3f98a 100644 --- a/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp @@ -3,28 +3,25 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) +#include "esp32_adc_driver.h" + #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - - -/** - * Inline adc reading implementation -*/ // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - // uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB, const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; } + #endif diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 0996ef11..8b932ae9 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,5 +1,6 @@ #include "../hardware_api.h" #include "../../drivers/hardware_api.h" +#include "../../drivers/hardware_specific/esp32_driver_mcpwm.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) @@ -15,85 +16,149 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - - /** * Inline adc reading implementation */ // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ +float _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = adcRead(pinA); - // uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB, const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; } + /** * Low side adc reading implementation */ static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; // Current readings from internal current sensor amplifiers int _pinA, _pinB, _pinC; -static void IRAM_ATTR isr_handler(void*); + +static void IRAM_ATTR mcpwm0_isr_handler(void*); +static void IRAM_ATTR mcpwm1_isr_handler(void*); byte currentState = 1; +int* adc_pins; +int* adc_buffer; +int adc_pin_count; +int adc_pin_read_index; // function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin){ +float _readADCVoltageLowSide(const int pin, const void* cs_params){ uint32_t raw_adc; if (pin == _pinA) raw_adc = a1; else if (pin == _pinB) raw_adc = a2; else if (pin == _pinC) raw_adc = a3; - return raw_adc * _ADC_CONV; + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - _pinA = pinA; - _pinB = pinB; - _pinC = pinC; +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + _pinA= pinA; + _pinB= pinB; + if( _isset(pinC) ) _pinC= pinC; + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; } -void _driverSyncLowSide(){ + +void _driverSyncLowSide(void* driver_params, void* cs_params){ // high side registers enable interrupt // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt // low-side register enable interrupt - MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt - mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler + mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; + mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + mcpwm_dev->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + if( _isset(_pinC) ) mcpwm_dev->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt + if(mcpwm_unit == MCPWM_UNIT_0) + mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler + else + mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm0_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; + // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; + // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; + + // low side + uint32_t mcpwm_intr_status_0 = MCPWM0.int_st.timer0_tep_int_st; + uint32_t mcpwm_intr_status_1 = MCPWM0.int_st.timer1_tep_int_st; + uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM0.int_st.timer2_tep_int_st : 0; + + switch (currentState) + { + case 1 : + if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); + currentState = 2; + break; + case 2 : + if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); + currentState = _isset(_pinC) ? 3 : 1; + break; + case 3 : + if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); + currentState = 1; + break; + } + + // high side + // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; + // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; + // low side + MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; + MCPWM0.int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; + if( _isset(_pinC) ) MCPWM0.int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } // Read currents when interrupt is triggered -static void IRAM_ATTR isr_handler(void*){ +static void IRAM_ATTR mcpwm1_isr_handler(void*){ // // high side // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; // low side - uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; - uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; - uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0; + uint32_t mcpwm_intr_status_0 = MCPWM1.int_st.timer0_tep_int_st; + uint32_t mcpwm_intr_status_1 = MCPWM1.int_st.timer1_tep_int_st; + uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM1.int_st.timer2_tep_int_st : 0; switch (currentState) { @@ -116,9 +181,10 @@ static void IRAM_ATTR isr_handler(void*){ // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; // low side - MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; - MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; - if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; + MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; + MCPWM1.int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; + if( _isset(_pinC) ) MCPWM1.int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } + #endif diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 50f383ad..c846aac6 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,31 +1,41 @@ #include "../hardware_api.h" // function reading an ADC value and returning the read voltage -__attribute__((weak)) float _readADCVoltageInline(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * 5.0f/1024.0f; + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ +__attribute__((weak)) void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (5.0f)/(1024.0f) + }; + + return params; } // function reading an ADC value and returning the read voltage -__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +__attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + return _readADCVoltageInline(pinA, cs_params); } // Configure low side for generic mcu // cannot do much but -__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); +__attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + return _configureADCInline(driver_params, pinA, pinB, pinC); } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(){ } +__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ + _UNUSED(driver_params); + _UNUSED(cs_params); +} __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp index 797ac6a1..cb4d9056 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -6,34 +6,20 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); -} + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); + return params; } -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32g4_mcu.cpp index 6fd22327..702a23ef 100644 --- a/src/current_sense/hardware_specific/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32g4_mcu.cpp @@ -3,8 +3,8 @@ #include "Arduino.h" #if defined(STM32G4xx) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4096.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f #define ADC_BUF_LEN_1 2 #define ADC_BUF_LEN_2 1 @@ -20,11 +20,9 @@ static DMA_HandleTypeDef hdma_adc2; uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin){ +float _readADCVoltageInline(const int pin, const void* cs_params){ uint32_t raw_adc = 0; if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[1]; @@ -34,11 +32,7 @@ float _readADCVoltageInline(const int pin){ else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[0]; #endif - return raw_adc * _ADC_CONV; -} -// do the same for low side sensing -float _readADCVoltageLowSide(const int pin){ - return _readADCVoltageInline(pin); + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ @@ -82,7 +76,9 @@ void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Chan __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); } -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + HAL_Init(); MX_GPIO_Init(); MX_DMA_Init(); @@ -111,12 +107,15 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. _delay(5); if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { - Error_Handler(); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } -} -// do the same for low side -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - _configureADCInline(pinA, pinB, pinC); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + + return params; } extern "C" { diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp index 606ce618..48f6f695 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -5,33 +5,20 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); -} + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + return params; } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32_driver_mcpwm.h new file mode 100644 index 00000000..56c1fb84 --- /dev/null +++ b/src/drivers/hardware_specific/esp32_driver_mcpwm.h @@ -0,0 +1,88 @@ +#ifndef ESP32_DRIVER_MCPWM_H +#define ESP32_DRIVER_MCPWM_H + +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +// empty motor slot +#define _EMPTY_SLOT -20 +#define _TAKEN_SLOT -21 + +// ABI bus frequency - would be better to take it from somewhere +// but I did nto find a good exposed variable +#define _MCPWM_FREQ 160e6f + +// preferred pwm resolution default +#define _PWM_RES_DEF 2048 +// min resolution +#define _PWM_RES_MIN 1500 +// max resolution +#define _PWM_RES_MAX 3000 +// pwm frequency +#define _PWM_FREQUENCY 25000 // default +#define _PWM_FREQUENCY_MAX 50000 // mqx + +// structure containing motor slot configuration +// this library supports up to 4 motors +typedef struct { + int pinA; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; + mcpwm_io_signals_t mcpwm_c; +} bldc_3pwm_motor_slots_t; + +typedef struct { + int pin1A; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_1a; + mcpwm_io_signals_t mcpwm_1b; + mcpwm_io_signals_t mcpwm_2a; + mcpwm_io_signals_t mcpwm_2b; +} stepper_4pwm_motor_slots_t; + +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + + +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; + mcpwm_dev_t* mcpwm_dev; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; +} ESP32MCPWMDriverParams; + + +#endif +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 6f6b5018..22719787 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,78 +1,7 @@ -#include "../hardware_api.h" +#include "esp32_driver_mcpwm.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// ABI bus frequency - would be better to take it from somewhere -// but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6f - -// preferred pwm resolution default -#define _PWM_RES_DEF 2048 -// min resolution -#define _PWM_RES_MIN 1500 -// max resolution -#define _PWM_RES_MAX 3000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_motor_slots_t; - -typedef struct { - int pin1A; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_1a; - mcpwm_io_signals_t mcpwm_1b; - mcpwm_io_signals_t mcpwm_2a; - mcpwm_io_signals_t mcpwm_2b; -} stepper_4pwm_motor_slots_t; - -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; - - - // define bldc motor slots array bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A @@ -102,18 +31,6 @@ stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { }; - -typedef struct ESP32MCPWMDriverParams { - long pwm_frequency; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; -} ESP32MCPWMDriverParams; - - - - - // configuring high frequency pwm timer // a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller @@ -237,6 +154,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, .mcpwm_unit = m_slot.mcpwm_unit, .mcpwm_operator1 = m_slot.mcpwm_operator }; @@ -288,6 +206,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, .mcpwm_unit = m_slot.mcpwm_unit, .mcpwm_operator1 = m_slot.mcpwm_operator }; @@ -344,6 +263,7 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, .mcpwm_unit = m_slot.mcpwm_unit, .mcpwm_operator1 = m_slot.mcpwm_operator1, .mcpwm_operator2 = m_slot.mcpwm_operator2 @@ -448,7 +368,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // return ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, - .mcpwm_unit = m_slot.mcpwm_unit + .mcpwm_dev = m_slot.mcpwm_num, + .mcpwm_unit = m_slot.mcpwm_unit }; return params; } From 452e1b8cc450872b135fc847b7ccf2a7937c7430 Mon Sep 17 00:00:00 2001 From: Daniel <16339876+dreusel@users.noreply.github.com> Date: Sat, 12 Feb 2022 16:20:54 +0100 Subject: [PATCH 056/474] pass shaft angle, not electric angle to motor.move in alignment_and_cogging_test --- .../alignment_and_cogging_test.ino | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 3fcd7b30..7324edf0 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -35,9 +35,10 @@ void testAlignmentAndCogging(int direction) { for (int i = 0; i < sample_count; i++) { - float electricAngle = (float) direction * i * motor.pole_pairs * shaft_rotation / sample_count; + float shaftAngle = (float) direction * i * shaft_rotation / sample_count; + float electricAngle = (float) shaftAngle * motor.pole_pairs; // move and wait - motor.move(electricAngle * PI / 180); + motor.move(shaftAngle * PI / 180); _delay(5); // measure From 0ec82e3e7f3f88addc29ef27da2ee83c3989d7d9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 23:38:43 +0100 Subject: [PATCH 057/474] New debug management independent of monitoring --- src/BLDCMotor.cpp | 1 + src/SimpleFOC.h | 1 + src/StepperMotor.cpp | 2 + src/communication/SimpleFOCDebug.cpp | 35 +++++ src/communication/SimpleFOCDebug.h | 24 +-- src/drivers/hardware_api.h | 2 + src/drivers/hardware_specific/samd21_mcu.cpp | 43 ++--- src/drivers/hardware_specific/samd51_mcu.cpp | 26 ++-- src/drivers/hardware_specific/samd_mcu.cpp | 156 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 6 +- 10 files changed, 168 insertions(+), 128 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 74b62bd9..03cec466 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,4 +1,5 @@ #include "BLDCMotor.h" +#include "./communication/SimpleFOCDebug.h" // BLDCMotor( int pp , float R) // - pp - pole pair number diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 3c24221c..606046d0 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -114,5 +114,6 @@ void loop() { #include "current_sense/GenericCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" +#include "communication/SimpleFOCDebug.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index e75f0743..d12f120c 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,4 +1,6 @@ #include "StepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + // StepperMotor(int pp) // - pp - pole pair number diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index 871fefc6..b89e5580 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -53,4 +53,39 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { } } + +void SimpleFOCDebug::print(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + + +void SimpleFOCDebug::print(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + + +void SimpleFOCDebug::print(int val) { + if (_debugPrint != NULL) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::print(float val) { + if (_debugPrint != NULL) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::println() { + if (_debugPrint != NULL) { + _debugPrint->println(); + } +} + #endif \ No newline at end of file diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index bc8a61b2..bccdf194 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -16,7 +16,7 @@ * * To produce debug output, use the macro SIMPLEFOC_DEBUG: * SIMPLEFOC_DEBUG("Debug message!"); - * SIMPLEFOC_DEBUG("a float value:", 123.456); + * SIMPLEFOC_DEBUG("a float value:", 123.456f); * SIMPLEFOC_DEBUG("an integer value: ", 123); * * Keep debugging output short and simple. Some of our MCUs have limited @@ -37,14 +37,20 @@ class SimpleFOCDebug { public: - void enable(Print* debugPrint = Serial); + static void enable(Print* debugPrint = &Serial); - void println(const __FlashStringHelper* msg); - void println(const char* msg); - void println(const __FlashStringHelper* msg, float val); - void println(const char* msg, float val); - void println(const __FlashStringHelper* msg, int val); - void println(const char* msg, int val); + static void println(const __FlashStringHelper* msg); + static void println(const char* msg); + static void println(const __FlashStringHelper* msg, float val); + static void println(const char* msg, float val); + static void println(const __FlashStringHelper* msg, int val); + static void println(const char* msg, int val); + static void println(); + + static void print(const char* msg); + static void print(const __FlashStringHelper* msg); + static void print(int val); + static void print(float val); protected: static Print* _debugPrint; @@ -52,7 +58,7 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ - SimpleFOCDebug::println(F(msg) __VA_OPT__(,) __VA_ARGS__) + SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 01f251b9..cb2385b7 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -3,6 +3,8 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../communication/SimpleFOCDebug.h" + // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index e4fb64b7..d9bb5b9a 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -164,7 +164,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); + SIMPLEFOC_DEBUG("SAMD: Configured clock..."); #endif } } @@ -227,8 +227,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: Initialized TC ", tccConfig.tcc.tccn); #endif } else { @@ -264,15 +263,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } } @@ -298,15 +298,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.bit.ENABLE = 1; while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 19474d03..a9191702 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -190,7 +190,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -292,9 +293,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51"); + SIMPLEFOC_DEBUG("SAMD: Not initialized: TC ", tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: TC units not supported on SAMD51"); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index b950fd09..4198cc9b 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -297,9 +297,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 2)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +306,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -326,7 +324,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif SAMDHardwareDriverParams* params = new SAMDHardwareDriverParams { @@ -388,7 +386,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -399,9 +397,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 3)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -412,7 +408,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -432,7 +428,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -469,7 +465,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -481,9 +477,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 4)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -496,7 +490,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -518,7 +512,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res; getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -575,7 +569,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -589,7 +583,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); + SIMPLEFOC_DEBUG("SAMD: Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -609,7 +603,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons if (!allAttached) return SIMPLEFOC_DRIVER_INIT_FAILED; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -637,7 +631,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res; getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -790,85 +784,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); + SimpleFOCDebug::println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); + SimpleFOCDebug::print(": TC Peripheral"); else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); + SimpleFOCDebug::print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; + SimpleFOCDebug::print(" E "); break; case PIO_TIMER_ALT: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; + SimpleFOCDebug::print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; + SimpleFOCDebug::print(" G "); break; #endif default: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; + SimpleFOCDebug::print(" ? "); break; } if (info.tcc.tccn>=0) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SimpleFOCDebug::print(info.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(info.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(info.wo); + SimpleFOCDebug::println("]"); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); + SimpleFOCDebug::println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 78f53664..181d8faf 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -3,11 +3,9 @@ #define SAMD_MCU_H -// uncomment to enable debug output to Serial port +// uncomment to enable debug output from SAMD driver +// can set this as build-flag in Arduino IDE or PlatformIO // #define SIMPLEFOC_SAMD_DEBUG -#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) -#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial -#endif #include "../hardware_api.h" From 00e550e2dbf8de59f6b350501989438f5115c713 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 23:45:24 +0100 Subject: [PATCH 058/474] make the old behaviour the default --- src/common/base_classes/FOCMotor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 922076a6..2763b709 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -1,4 +1,5 @@ #include "FOCMotor.h" +#include "../../communication/SimpleFOCDebug.h" /** * Default constructor - setting all variabels to default values @@ -77,7 +78,8 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print - if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); + SimpleFOCDebug::enable(&print); + SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); } // utility function intended to be used with serial plotter to monitor motor variables From 4441d36dad8b24f7900ba81c44e7bd9c149ba056 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 13 Feb 2022 16:40:11 +0100 Subject: [PATCH 059/474] esp32 adc driver speedup --- .../hardware_specific/esp32_adc_driver.cpp | 40 ++++++- .../hardware_specific/esp32_mcu.cpp | 106 ++++++++---------- 2 files changed, 81 insertions(+), 65 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 40dc0140..93aba630 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -216,10 +216,44 @@ void __analogReadResolution(uint8_t bits) uint16_t IRAM_ATTR adcRead(uint8_t pin) { - if(!__adcAttachPin(pin) || !__adcStart(pin)){ - return 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin } - return __adcEnd(pin); + + __analogInit(); + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + } + + uint16_t value = 0; + + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 8b932ae9..1589fe4b 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -95,15 +95,11 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p void _driverSyncLowSide(void* driver_params, void* cs_params){ // high side registers enable interrupt // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt // low-side register enable interrupt mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - mcpwm_dev->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - if( _isset(_pinC) ) mcpwm_dev->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt if(mcpwm_unit == MCPWM_UNIT_0) mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler else @@ -113,77 +109,63 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm0_isr_handler(void*){ // // high side - // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; - // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; - // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; - + // uint32_t mcpwm_intr_status_0 = MCPWM0.int_st.timer0_tez_int_st; + // low side - uint32_t mcpwm_intr_status_0 = MCPWM0.int_st.timer0_tep_int_st; - uint32_t mcpwm_intr_status_1 = MCPWM0.int_st.timer1_tep_int_st; - uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM0.int_st.timer2_tep_int_st : 0; - - switch (currentState) - { - case 1 : - if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); - currentState = 2; - break; - case 2 : - if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); - currentState = _isset(_pinC) ? 3 : 1; - break; - case 3 : - if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); - currentState = 1; - break; + uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + switch (currentState) + { + case 1 : + a1 = adcRead(_pinA); + currentState = 2; + break; + case 2 : + a2 = adcRead(_pinB); + currentState = _isset(_pinC) ? 3 : 1; + break; + case 3 : + a3 = adcRead(_pinC); + currentState = 1; + break; + } } - // high side - // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; - // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; - // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; + // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; - MCPWM0.int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; - if( _isset(_pinC) ) MCPWM0.int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; + MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; } // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm1_isr_handler(void*){ // // high side - // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; - // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; - // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; - + // uint32_t mcpwm_intr_status_0 = MCPWM1.int_st.timer0_tez_int_st; + // low side - uint32_t mcpwm_intr_status_0 = MCPWM1.int_st.timer0_tep_int_st; - uint32_t mcpwm_intr_status_1 = MCPWM1.int_st.timer1_tep_int_st; - uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM1.int_st.timer2_tep_int_st : 0; - - switch (currentState) - { - case 1 : - if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); - currentState = 2; - break; - case 2 : - if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); - currentState = _isset(_pinC) ? 3 : 1; - break; - case 3 : - if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); - currentState = 1; - break; + uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + switch (currentState) + { + case 1 : + a1 = adcRead(_pinA); + currentState = 2; + break; + case 2 : + a2 = adcRead(_pinB); + currentState = _isset(_pinC) ? 3 : 1; + break; + case 3 : + a3 = adcRead(_pinC); + currentState = 1; + break; + } } - // high side - // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; - // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; - // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; + // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; - MCPWM1.int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; - if( _isset(_pinC) ) MCPWM1.int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; + MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; } From 6019e9d33ab6b9212125a644932a5ce0ba720d0b Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 21 Feb 2022 11:55:10 +0100 Subject: [PATCH 060/474] esp32 final support for multiple motors + samd refactor for new low level api --- .../hardware_specific/esp32_mcu.cpp | 122 ++++++++---------- .../hardware_specific/samd21_mcu.cpp | 27 ++-- .../hardware_specific/samd_mcu.cpp | 34 ++--- 3 files changed, 88 insertions(+), 95 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 1589fe4b..6080f243 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -16,24 +16,33 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f + +typedef struct ESP32MCPWMCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + mcpwm_unit_t mcpwm_unit; + int buffer_index; +} ESP32MCPWMCurrentSenseParams; + + /** * Inline adc reading implementation */ // function reading an ADC value and returning the read voltage float _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; + return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ _UNUSED(driver_params); pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; @@ -46,46 +55,51 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi /** * Low side adc reading implementation */ -static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; -int a1, a2, a3; // Current readings from internal current sensor amplifiers -int _pinA, _pinB, _pinC; static void IRAM_ATTR mcpwm0_isr_handler(void*); static void IRAM_ATTR mcpwm1_isr_handler(void*); byte currentState = 1; - -int* adc_pins; -int* adc_buffer; -int adc_pin_count; -int adc_pin_read_index; - +// two mcpwm units +// - max 2 motors per mcpwm unit (6 adc channels) +int adc_pins[2][6]={0}; +int adc_pin_count[2]={0}; +uint32_t adc_buffer[2][6]={0}; +int adc_read_index[2]={0}; // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ uint32_t raw_adc; - if (pin == _pinA) raw_adc = a1; - else if (pin == _pinB) raw_adc = a2; - else if (pin == _pinC) raw_adc = a3; + mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; + int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; + float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; + for(int i=0; i < adc_pin_count[unit]; i++){ + if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; + } + // not found + return 0; } -// function reading an ADC value and returning the read voltage +// function configuring low-side current sensing void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); - - _pinA= pinA; - _pinB= pinB; - if( _isset(pinC) ) _pinC= pinC; + + mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + int index_start = adc_pin_count[unit]; + adc_pins[unit][adc_pin_count[unit]++] = pinA; + adc_pins[unit][adc_pin_count[unit]++] = pinB; + if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), + .mcpwm_unit = unit, + .buffer_index = index_start }; return params; @@ -93,13 +107,16 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p void _driverSyncLowSide(void* driver_params, void* cs_params){ - // high side registers enable interrupt - // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // low-side register enable interrupt mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + + // low-side register enable interrupt mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // high side registers enable interrupt + //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt + + // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) if(mcpwm_unit == MCPWM_UNIT_0) mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler else @@ -109,63 +126,38 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm0_isr_handler(void*){ // // high side - // uint32_t mcpwm_intr_status_0 = MCPWM0.int_st.timer0_tez_int_st; + // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; // low side uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; if(mcpwm_intr_status){ - switch (currentState) - { - case 1 : - a1 = adcRead(_pinA); - currentState = 2; - break; - case 2 : - a2 = adcRead(_pinB); - currentState = _isset(_pinC) ? 3 : 1; - break; - case 3 : - a3 = adcRead(_pinC); - currentState = 1; - break; - } + adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); + adc_read_index[0]++; + if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; } - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; - // low side MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; } + // Read currents when interrupt is triggered static void IRAM_ATTR mcpwm1_isr_handler(void*){ // // high side - // uint32_t mcpwm_intr_status_0 = MCPWM1.int_st.timer0_tez_int_st; + // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; // low side uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; if(mcpwm_intr_status){ - switch (currentState) - { - case 1 : - a1 = adcRead(_pinA); - currentState = 2; - break; - case 2 : - a2 = adcRead(_pinB); - currentState = _isset(_pinC) ? 3 : 1; - break; - case 3 : - a3 = adcRead(_pinC); - currentState = 1; - break; - } + adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); + adc_read_index[1]++; + if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; } - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; - // low side MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; } diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 7d742a2e..e72a054d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -8,21 +8,23 @@ static bool freeRunning = false; static int _pinA, _pinB, _pinC; static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - adc pin A - * @param pinB - adc pin B - * @param pinC - adc pin C - */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC) + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC) { + _UNUSED(driver_params); + _pinA = pinA; _pinB = pinB; _pinC = pinC; freeRunning = true; instance.init(pinA, pinB, pinC); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC } + }; + + return params; } void _startADC3PinConversionLowSide() { @@ -33,8 +35,10 @@ void _startADC3PinConversionLowSide() * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltageLowSide(const int pinA) +float _readADCVoltageLowSide(const int pinA, const void* cs_params) { + _UNUSED(cs_params); + instance.readResults(a, b, c); if(pinA == _pinA) @@ -50,8 +54,11 @@ float _readADCVoltageLowSide(const int pinA) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide() +void _driverSyncLowSide(void* driver_params, void* cs_params) { + _UNUSED(driver_params); + _UNUSED(cs_params); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp index 550d7bee..d693ce15 100644 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -1,37 +1,31 @@ #include "../hardware_api.h" -#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) +#if defined(_SAMD21_)|| defined(_SAMD51_) || defined(_SAME51_) #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ +float _readADCVoltageInline(const int pinA, const void* cs_params){ + digitalWrite(7,HIGH); uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + digitalWrite(7,LOW); + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } + // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + pinMode(7,OUTPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); -} + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); + return params; } -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - #endif \ No newline at end of file From 0994853f120204e2f19c158b5ea89159f10c6d3f Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 21 Feb 2022 17:05:00 +0100 Subject: [PATCH 061/474] forgotten debugging code --- src/current_sense/hardware_specific/samd_mcu.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp index d693ce15..d4771318 100644 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -5,19 +5,10 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - digitalWrite(7,HIGH); - uint32_t raw_adc = analogRead(pinA); - digitalWrite(7,LOW); - return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); - pinMode(7,OUTPUT); - pinMode(pinA, INPUT); + pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); From 92915ff27c7a5cc51e68cb80b144c7a1d600edc3 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Feb 2022 15:04:59 +0100 Subject: [PATCH 062/474] generic current sense typo --- src/current_sense/GenericCurrentSense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index bd1a4ddc..18cf8da4 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -41,9 +41,9 @@ void GenericCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current = readCallback(); - current.a = (current.a - offset_ia);// amps - current.b = (current.a - offset_ib);// amps - current.c = (current.a - offset_ic); // amps + current.a = (current.a - offset_ia); // amps + current.b = (current.b - offset_ib); // amps + current.c = (current.c - offset_ic); // amps return current; } From 358f1e334431a79da15757168d1703a64ceaa977 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Feb 2022 15:06:13 +0100 Subject: [PATCH 063/474] added example for odrive --- .../odrive_example/odrive_example.ino | 112 ++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 examples/hardware_specific_examples/odrive_example/odrive_example.ino diff --git a/examples/hardware_specific_examples/odrive_example/odrive_example.ino b/examples/hardware_specific_examples/odrive_example/odrive_example.ino new file mode 100644 index 00000000..bc473224 --- /dev/null +++ b/examples/hardware_specific_examples/odrive_example/odrive_example.ino @@ -0,0 +1,112 @@ +/* + Odrive robotics' hardware is one of the best BLDC motor foc supporting hardware out there. + + This is an example code that can be directly uploaded to the Odrive usind the SWD programmer. + This code uses an encoder with 500 cpr and a BLDC motor with 7 pole pairs connected to the M0 interface of the Odrive. + + This is a short template code and the idea is that you are able to adapt to your needs not to be a complete solution. :D +*/ +#include + +// Odrive M0 motor pinout +#define M0_INH_A PA8 +#define M0_INH_B PA9 +#define M0_INH_C PA10 +#define M0_INL_A PB13 +#define M0_INL_B PB14 +#define M0_INL_C PB15 +// M0 currnets +#define M0_IB PC0 +#define M0_IC PC1 +// Odrive M0 encoder pinout +#define M0_ENC_A PB4 +#define M0_ENC_B PB5 + + +// Odrive M1 motor pinout +#define M1_INH_A PC6 +#define M1_INH_B PC7 +#define M1_INH_C PC8 +#define M1_INL_A PA7 +#define M1_INL_B PB0 +#define M1_INL_C PB1 +// M0 currnets +#define M0_IB PC2 +#define M0_IC PC3 +// Odrive M1 encoder pinout +#define M1_ENC_A PB6 +#define M1_ENC_B PB7 + +// M1 & M2 common enable pin +#define EN_GATE PB12 + + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver6PWM driver = BLDCDriver6PWM(M0_INH_A,M0_INL_A, M0_INH_B,M0_INL_B, M0_INH_C,M0_INL_C, EN_GATE); + +// instantiate the commander +Commander command = Commander(Serial); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + + +Encoder encoder = Encoder(M0_ENC_A, M0_ENC_B, 500); +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +void setup(){ + + // pwm frequency to be used [Hz] + driver.pwm_frequency = 20000; + // power supply voltage [V] + driver.voltage_power_supply = 20; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 20; + // driver init + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.voltage_sensor_align = 0.5; + + // max voltage allowed for motion control + motor.voltage_limit = 8.0; + // alignment voltage limit + motor.voltage_sensor_align = 0.5; + + + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_downsample = 0; // disable monitoring at start + + // add target command T + command.add('M', doMotor, "motor M0"); + + // initialise motor + motor.init(); + // init FOC + motor.initFOC(); + delay(1000); +} +void loop(){ + // foc loop + motor.loopFOC(); + // motion control + motor.move(); + // monitoring + motor.monitor(); + // user communication + command.run(); +} \ No newline at end of file From 9a94b34351de993edbb5ed886fc19eedbdd3720c Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Feb 2022 15:10:49 +0100 Subject: [PATCH 064/474] new version increment --- README.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index c6c47b22..267cbc5d 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Therefore this is an attempt to:
    -

    NEW RELEASE 📢: SimpleFOClibrary v2.2.1 see release

    +

    NEXT RELEASE 📢: SimpleFOClibrary v2.2.2 see release