From a71f12ca77e3e53a6f9766138c0309b882ea335b Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 11:12:36 -0600 Subject: [PATCH 001/103] Samd support add support for serial insterface other than Serial I use -D SIMPLEFOC_SAMD_DEBUG_SERIAL=SERIAL_PORT_MONITOR --- src/drivers/hardware_specific/samd21_mcu.cpp | 34 ++-- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +-- src/drivers/hardware_specific/samd_mcu.cpp | 162 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 5 +- 4 files changed, 112 insertions(+), 109 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ 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 - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 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 - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,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 - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("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 @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + 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("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" From d37f34ebeb0d9fa75e39c5302144d1564ffd1fb1 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 10:18:17 +0200 Subject: [PATCH 002/103] initial low side added --- src/current_sense/InlineCurrentSense.cpp | 14 +- src/current_sense/LowsideCurrentSense.cpp | 179 ++++++++++++++++++ src/current_sense/LowsideCurrentSense.h | 59 ++++++ src/current_sense/hardware_api.h | 23 ++- .../hardware_specific/esp32_mcu.cpp | 80 ++++++++ .../hardware_specific/generic_mcu.cpp | 8 +- 6 files changed, 350 insertions(+), 13 deletions(-) create mode 100644 src/current_sense/LowsideCurrentSense.cpp create mode 100644 src/current_sense/LowsideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/esp32_mcu.cpp diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 2e9c4421..879395d1 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -22,7 +22,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // Inline sensor init function void InlineCurrentSense::init(){ // configure ADC variables - _configureADC(pinA,pinB,pinC); + _configureADCInline(pinA,pinB,pinC); // calibrate zero offsets calibrateOffsets(); } @@ -36,9 +36,9 @@ void InlineCurrentSense::calibrateOffsets(){ offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltage(pinA); - offset_ib += _readADCVoltage(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + offset_ia += _readADCVoltageInline(pinA); + offset_ib += _readADCVoltageInline(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); _delay(1); } // calculate the mean offsets @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + 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 return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp new file mode 100644 index 00000000..abebf03d --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -0,0 +1,179 @@ +#include "LowsideCurrentSense.h" +// LowsideCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Lowside sensor init function +void LowsideCurrentSense::init(){ + // configure ADC variables + _configureADCLowSide(pinA,pinB,pinC); + // calibrate zero offsets + calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowsideCurrentSense::calibrateOffsets(){ + // 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 < 1000; i++) { + offset_ia += _readADCVoltageLowSide(pinA); + offset_ib += _readADCVoltageLowSide(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / 1000.0; + offset_ib = offset_ib / 1000.0; + if(_isset(pinC)) offset_ic = offset_ic / 1000.0; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + 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 + 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 +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + //gain_a *= -1; + //gain_b *= -1; + //gain_c *= -1; + + /* + 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(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // 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.7 ){ // 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.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // 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.7 ){ // 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.0; + } + 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; + */ + return 1; +} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h new file mode 100644 index 00000000..2300d5f6 --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.h @@ -0,0 +1,59 @@ +#ifndef LOWSIDE_CS_LIB_H +#define LOWSIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/FOCMotor.h" +#include "hardware_api.h" + + +class LowsideCurrentSense: public CurrentSense{ + public: + /** + LowsideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ab376e8e..4c4c1086 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -9,7 +9,7 @@ * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltage(const int pinA); +float _readADCVoltageInline(const int pinA); /** * function reading an ADC value and returning the read voltage @@ -18,6 +18,25 @@ float _readADCVoltage(const int pinA); * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); +void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); +/** + * 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 = NOT_SET); +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA); + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide(); #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 new file mode 100644 index 00000000..7f84f9a9 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,80 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 4095.0 + +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*); +byte currentState = 1; + +// 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 _readADCVoltageLowSide(const int pin){ + 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; +} + + +// 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; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + +void _driverSyncLowSide(){ + 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 + //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 +} + +// Read currents when interrupt is triggered +static void IRAM_ATTR isr_handler(void*){ + 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 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + + if(mcpwm_intr_status_0 > 0 && currentState == 1){ + a1 = analogRead(_pinA); + //a2 = analogRead(_pinB); + currentState = 2; + } + else if(mcpwm_intr_status_1 > 0 && currentState == 2){ + a2 = analogRead(_pinB); + //a3 = analogRead(_pinC); + currentState = 1; + } + /* + else if(mcpwm_intr_status_2 > 0 && currentState == 3){ + a3 = analogRead(_pinC); + //a1 = analogRead(_pinA); + currentState = 1; + }*/ + + 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; + //MCPWM[MCPWM_UNIT_0]->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 423774c2..dccd3601 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -18,8 +18,8 @@ #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 #elif defined(ESP_H) // or esp32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 4095.0 + // do nothing implemented in esp32_mcu.h + #elif defined(_STM32_DEF_) // or stm32 #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 @@ -33,14 +33,14 @@ #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) // function reading an ADC value and returning the read voltage -float _readADCVoltage(const int pinA){ +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 _configureADC(const int pinA,const int pinB,const int pinC){ +void _configureADCInline(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); From c21dc2e22df6b4e9d8fffbbf9df8b6e25fd1d234 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 19:19:06 -0600 Subject: [PATCH 003/103] LowSideCurrentSense: first (truly) successful DMA+ADC readings, 1.65-centered readings with the drv8305, just as intented --- src/current_sense/LowSideCurrentSense.cpp | 177 ++++++++++++ src/current_sense/LowSideCurrentSense.h | 57 ++++ src/current_sense/hardware_api.h | 4 + .../hardware_specific/samd21_mcu.cpp | 266 ++++++++++++++++++ 4 files changed, 504 insertions(+) create mode 100644 src/current_sense/LowSideCurrentSense.cpp create mode 100644 src/current_sense/LowSideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp new file mode 100644 index 00000000..f9647f7b --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -0,0 +1,177 @@ +#include "LowSideCurrentSense.h" +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Inline sensor init function +void LowSideCurrentSense::init(){ + // configure ADC variables + _configure3PinsDMA(pinA,pinB,pinC); + _start3PinsDMA(); //start next acuisition + // calibrate zero offsets + // calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowSideCurrentSense::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 += _readADCVoltage(pinA); + offset_ib += _readADCVoltage(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps + // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps + // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + _start3PinsDMA(); + + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowSideCurrentSense::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 LowSideCurrentSense::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.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // 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.7 ){ // 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.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // 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.7 ){ // 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.0; + } + 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/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h new file mode 100644 index 00000000..48c6dc80 --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.h @@ -0,0 +1,57 @@ +#ifndef LOW_SIDE_CS_LIB_H +#define LOW_SIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "hardware_api.h" + +class LowSideCurrentSense: public CurrentSense{ + public: + /** + LowSideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ab376e8e..83377820 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,4 +20,8 @@ float _readADCVoltage(const int pinA); */ void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); +void _start3PinsDMA(); +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC = NOT_SET); + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..fb0dbeaf --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,266 @@ + +#include "../hardware_api.h" + +// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless +static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout +static uint32_t ADC_FirstPin; // PA04 +static uint32_t ADC_LastPin; // PA06 +static uint32_t BufferSize = 0; + +uint16_t adcBuffer[20]; + + +static uint32_t ADC_pinA = A4; +static uint32_t ADC_pinB = A5; +static uint32_t ADC_pinC = 8; + + +#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val +#define _ADC_RESOLUTION (1 << 12) +#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) + +void adc_dma(void *rxdata, size_t hwords); +void adc_init(); +void adc_start_with_DMA(); +void dma_init(); + +void _start3PinsDMA() +{ + adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); + adc_start_with_DMA(); +} +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +{ + while(ADC->CTRLA.bit.ENABLE) ; + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[adcA] * ADC_CONV_; + b = adcBuffer[adcB] * ADC_CONV_; + if(_isset(pinC)) + { + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[adcC] * ADC_CONV_; + } +} + +// function reading an ADC value and returning the read voltage +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ + + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + + pinMode(42, INPUT); + ADC_FirstPin = min(adcA, adcB); + ADC_LastPin = max(adcA, adcB); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) + { + pinMode(pinC, INPUT); + ADC_FirstPin = min(ADC_FirstPin, adcC); + ADC_LastPin = max(ADC_LastPin, adcC); + } + + ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout + BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + + ADC_pinA = pinA; + ADC_pinB = pinB; + ADC_pinC = pinC; + + // ADC and DMA + adc_init(); + dma_init(); +} + + + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) + +static uint32_t ADC_DMA_chnl = 3; // DMA channel + + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +/** + * @brief Initialize ADC + * @retval void + */ +void adc_init(){ + + auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //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; + ADCsync(); // ref 31.6.16 + + a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + SerialUSB.println("--------------"); + SerialUSB.println(a); + SerialUSB.println(b); + SerialUSB.println(c); + SerialUSB.println("--------------"); +/* +Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan +This register gives the number of input sources included in the pin scan. The number of input sources included is +INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + +INPUTOFFSET + INPUTSCAN. +The range of the scan mode must not exceed the number of input channels available on the device. +Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection +These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If +the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit +group in the SamplingControl register must be written. +Table 32-14. Positive Mux Input Selection +MUXPOS[4:0] Group configuration Description +0x00 PIN0 ADC AIN0 pin +0x01 PIN1 ADC AIN1 pin +0x02 PIN2 ADC AIN2 pin +0x03 PIN3 ADC AIN3 pin +0x04 PIN4 ADC AIN4 pin +0x05 PIN5 ADC AIN5 pin +0x06 PIN6 ADC AIN6 pin +0x07 PIN7 ADC AIN7 pin +0x08 PIN8 ADC AIN8 pin +0x09 PIN9 ADC AIN9 pin +0x0A PIN10 ADC AIN10 pin +0x0B PIN11 ADC AIN11 pin +0x0C PIN12 ADC AIN12 pin +0x0D PIN13 ADC AIN13 pin +0x0E PIN14 ADC AIN14 pin +0x0F PIN15 ADC AIN15 pin +0x10 PIN16 ADC AIN16 pin +0x11 PIN17 ADC AIN17 pin +0x12 PIN18 ADC AIN18 pin +0x13 PIN19 ADC AIN19 pin +0x14-0x17 Reserved +0x18 TEMP Temperature reference +0x19 BANDGAP Bandgap voltage +0x1A SCALEDCOREVCC 1/4 scaled core supply +0x1B SCALEDIOVCC 1/4 scaled I/O supply +0x1C DAC DAC output +0x1D-0x1F Reserved +*/ + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + + +/** + * @brief dma_init + * @retval void + */ +void dma_init() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + +/** + * @brief adc_dma + * @retval void + */ +void adc_dma(void *rxdata, size_t hwords) { + uint32_t temp_CHCTRLB_reg; + + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); + temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | +DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + +/** + * @brief adc_stop_with_DMA + * @retval void + */ +void adc_stop_with_DMA(void) +{ + ADC->CTRLA.bit.ENABLE = 0x00; + // SerialUSB.println("DMA stopped!"); +} + +/** + * @brief adc_start_with_DMA + * @retval void + */ +void adc_start_with_DMA(void) +{ + // SerialUSB.println("strating DMA..."); + + + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADC->SWTRIG.bit.FLUSH = 1; + ADC->CTRLA.bit.ENABLE = 0x01; +} + + +/** + * @brief DMAC_Handler + * @retval void + */ +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adc_stop_with_DMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; +} From e6d77b08e983ae4e3bfb2640a558f29d153dbf81 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 20:26:26 -0600 Subject: [PATCH 004/103] wrapped samd21 adc/dma implementation in an ugly class --- src/SimpleFOC.h | 1 + src/current_sense/LowSideCurrentSense.cpp | 13 +- src/current_sense/LowSideCurrentSense.h | 3 + .../hardware_specific/samd21_mcu.cpp | 208 ++++++++---------- .../hardware_specific/samd21_mcu.h | 60 +++++ 5 files changed, 168 insertions(+), 117 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index f9647f7b..0c12e528 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -5,7 +5,9 @@ // - phA - A phase adc pin // - phB - B phase adc pin // - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): +adc(_pinA, _pinB, _pinC) +{ pinA = _pinA; pinB = _pinB; pinC = _pinC; @@ -21,9 +23,7 @@ LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int // Inline sensor init function void LowSideCurrentSense::init(){ - // configure ADC variables - _configure3PinsDMA(pinA,pinB,pinC); - _start3PinsDMA(); //start next acuisition + adc.init(); // calibrate zero offsets // calibrateOffsets(); } @@ -54,8 +54,9 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - _start3PinsDMA(); + + adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + adc._start3PinsDMA(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 48c6dc80..2a898bfc 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -6,6 +6,7 @@ #include "../common/time_utils.h" #include "../common/base_classes/CurrentSense.h" #include "hardware_api.h" +#include "hardware_specific/samd21_mcu.h" class LowSideCurrentSense: public CurrentSense{ public: @@ -52,6 +53,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) + SAMDCurrentSensceADC adc; + }; #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index fb0dbeaf..c95bb754 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,35 +1,57 @@ -#include "../hardware_api.h" +#include "samd21_mcu.h" -// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout -static uint32_t ADC_FirstPin; // PA04 -static uint32_t ADC_LastPin; // PA06 -static uint32_t BufferSize = 0; -uint16_t adcBuffer[20]; +void adc_stop_with_DMA(void); +void adc_start_with_DMA(void); +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -static uint32_t ADC_pinA = A4; -static uint32_t ADC_pinB = A5; -static uint32_t ADC_pinC = 8; -#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val -#define _ADC_RESOLUTION (1 << 12) -#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) +SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) +: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) +{ + ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; +} + +void SAMDCurrentSensceADC::init() +{ + _configure3PinsDMA(); + _start3PinsDMA(); //s +} -void adc_dma(void *rxdata, size_t hwords); -void adc_init(); -void adc_start_with_DMA(); -void dma_init(); -void _start3PinsDMA() +void SAMDCurrentSensceADC::_start3PinsDMA() { adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); adc_start_with_DMA(); } -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) { while(ADC->CTRLA.bit.ENABLE) ; uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; @@ -44,7 +66,7 @@ void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, floa } // function reading an ADC value and returning the read voltage -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ +void SAMDCurrentSensceADC::_configure3PinsDMA(){ uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; @@ -65,10 +87,6 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; - ADC_pinA = pinA; - ADC_pinB = pinB; - ADC_pinC = pinC; - // ADC and DMA adc_init(); dma_init(); @@ -76,41 +94,17 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ -// ADC DMA sequential free running (6) with Interrupts ///////////////// - -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) - -static uint32_t ADC_DMA_chnl = 3; // DMA channel -/** - * @brief ADC sync wait - * @retval void - */ -static __inline__ void ADCsync() __attribute__((always_inline, unused)); -static void ADCsync() { - while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free -} - /** * @brief Initialize ADC * @retval void */ -void adc_init(){ +void SAMDCurrentSensceADC::adc_init(){ - auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); @@ -121,54 +115,46 @@ void adc_init(){ // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 - a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - c = analogRead(ADC_pinC); // do some pin init pinPeripheral() - SerialUSB.println("--------------"); - SerialUSB.println(a); - SerialUSB.println(b); - SerialUSB.println(c); - SerialUSB.println("--------------"); -/* -Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan -This register gives the number of input sources included in the pin scan. The number of input sources included is -INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + -INPUTOFFSET + INPUTSCAN. -The range of the scan mode must not exceed the number of input channels available on the device. -Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection -These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If -the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit -group in the SamplingControl register must be written. -Table 32-14. Positive Mux Input Selection -MUXPOS[4:0] Group configuration Description -0x00 PIN0 ADC AIN0 pin -0x01 PIN1 ADC AIN1 pin -0x02 PIN2 ADC AIN2 pin -0x03 PIN3 ADC AIN3 pin -0x04 PIN4 ADC AIN4 pin -0x05 PIN5 ADC AIN5 pin -0x06 PIN6 ADC AIN6 pin -0x07 PIN7 ADC AIN7 pin -0x08 PIN8 ADC AIN8 pin -0x09 PIN9 ADC AIN9 pin -0x0A PIN10 ADC AIN10 pin -0x0B PIN11 ADC AIN11 pin -0x0C PIN12 ADC AIN12 pin -0x0D PIN13 ADC AIN13 pin -0x0E PIN14 ADC AIN14 pin -0x0F PIN15 ADC AIN15 pin -0x10 PIN16 ADC AIN16 pin -0x11 PIN17 ADC AIN17 pin -0x12 PIN18 ADC AIN18 pin -0x13 PIN19 ADC AIN19 pin -0x14-0x17 Reserved -0x18 TEMP Temperature reference -0x19 BANDGAP Bandgap voltage -0x1A SCALEDCOREVCC 1/4 scaled core supply -0x1B SCALEDIOVCC 1/4 scaled I/O supply -0x1C DAC DAC output -0x1D-0x1F Reserved -*/ + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; ADCsync(); ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] @@ -176,7 +162,8 @@ MUXPOS[4:0] Group configuration Description ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; ADCsync(); @@ -187,7 +174,7 @@ MUXPOS[4:0] Group configuration Description * @brief dma_init * @retval void */ -void dma_init() { +void SAMDCurrentSensceADC::dma_init() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -201,7 +188,7 @@ void dma_init() { * @brief adc_dma * @retval void */ -void adc_dma(void *rxdata, size_t hwords) { +void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { uint32_t temp_CHCTRLB_reg; DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); @@ -209,7 +196,7 @@ void adc_dma(void *rxdata, size_t hwords) { DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | -DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -224,6 +211,9 @@ DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } + + + /** * @brief adc_stop_with_DMA * @retval void @@ -241,16 +231,12 @@ void adc_stop_with_DMA(void) void adc_start_with_DMA(void) { // SerialUSB.println("strating DMA..."); - - - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } - - /** * @brief DMAC_Handler * @retval void @@ -263,4 +249,4 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..fb5d319a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,60 @@ + +#include "../hardware_api.h" + + +class SAMDCurrentSensceADC +{ + +public: + + SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + + void init(); + // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout + uint32_t ADC_FirstPin; // PA04 + uint32_t ADC_LastPin; // PA06 + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA = A4; + uint32_t pinB = A5; + uint32_t pinC = 8; + + float _ADC_VOLTAGE; + float _ADC_RESOLUTION; + float ADC_CONV_; + + void _start3PinsDMA(); + void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); + // function reading an ADC value and returning the read voltage + void _configure3PinsDMA(); + + + + + uint32_t ADC_DMA_chnl = 3; // DMA channel + + + /** + * @brief Initialize ADC + * @retval void + */ + void adc_init(); + + + /** + * @brief dma_init + * @retval void + */ + void dma_init(); + + /** + * @brief adc_dma + * @retval void + */ + void adc_dma(void *rxdata, size_t hwords); + +}; From 0b7839f1e94356d4dde5757ddef412cce52d8331 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 May 2021 13:05:37 +0200 Subject: [PATCH 005/103] FEAT added weak definitions of generic hardware implementations + arduino due same frequency PWM and TC calculation --- src/common/foc_utils.h | 1 + .../hardware_specific/atmega328_mcu.cpp | 7 +- src/drivers/hardware_specific/due_mcu.cpp | 87 +++++++++++-------- src/drivers/hardware_specific/generic_mcu.cpp | 70 ++++++++------- src/drivers/hardware_specific/teensy_mcu.cpp | 14 --- 5 files changed, 95 insertions(+), 84 deletions(-) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 06978424..30e99b4f 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -9,6 +9,7 @@ #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) +#define _UNUSED(v) (void) (v) // utility defines #define _2_SQRT3 1.15470053838 diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index cf12c302..be8dadb1 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -6,7 +6,7 @@ void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { + if (pin == 5 || pin == 6 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } @@ -17,12 +17,12 @@ void _pinHighFrequency(const int pin){ } - // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -34,6 +34,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - hardware speciffic // supports Arudino/ATmega328 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -118,6 +119,8 @@ int _configureComplementaryPair(int pinH, int pinL) { // - 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); // High PWM frequency // - always max 32kHz int ret_flag = 0; diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 27097301..e0d39afd 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -1,6 +1,11 @@ #include "../hardware_api.h" -#if defined(__arm__) && defined(__SAM3X8E__) +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +#define _PWM_RES_MIN 255 // 50khz // pwm frequency and max duty cycle static unsigned long _pwm_frequency; @@ -101,7 +106,29 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if (!PWMEnabled) { // PWM Startup code pmc_enable_periph_clk(PWM_INTERFACE_ID); - PWMC_ConfigureClocks(pwm_freq * _max_pwm_value, 0, VARIANT_MCK); + // this function does not work too well - I'll rewrite it + // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK); + + // finding the divisors an prescalers form FindClockConfiguration function + uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024}; + uint8_t divisor = 0; + uint32_t prescaler; + + /* Find prescaler and divisor values */ + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + while ((prescaler > 255) && (divisor < 11)) { + divisor++; + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + } + // update the divisor*prescaler value + prescaler = prescaler | (divisor << 8); + + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler)) + _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler); + // set the prescaler value + PWM->PWM_CLK = prescaler; + PWMEnabled = 1; } @@ -112,6 +139,8 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ g_APinDescription[ulPin].ulPinType, g_APinDescription[ulPin].ulPin, g_APinDescription[ulPin].ulPinConfiguration); + // PWM_CMR_CALG - center align + // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0); PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0); PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value); PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0); @@ -123,7 +152,7 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin // We use MCLK/2 as clock. - const uint32_t TC = VARIANT_MCK / 2 / pwm_freq; + const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ; // Setup Timer for this pin ETCChannel channel = g_APinDescription[ulPin].ulTCChannel; uint32_t chNo = channelToChNo[channel]; @@ -131,17 +160,18 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ Tc *chTC = channelToTC[channel]; uint32_t interfaceID = channelToId[channel]; - if (!TCChanEnabled[interfaceID]) { - pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); - TC_Configure(chTC, chNo, - TC_CMR_TCCLKS_TIMER_CLOCK1 | - TC_CMR_WAVE | // Waveform mode - TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC - TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) - TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | - TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); - TC_SetRC(chTC, chNo, TC); - } + if (!TCChanEnabled[interfaceID]) { + pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); + TC_Configure(chTC, chNo, + TC_CMR_TCCLKS_TIMER_CLOCK1 | + TC_CMR_WAVE | // Waveform mode + TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC + TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) + TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | + TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); + TC_SetRC(chTC, chNo, TC); + } + // disable the counter on start if (chA){ TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET); @@ -197,7 +227,7 @@ void setPwm(uint32_t ulPin, uint32_t ulValue) { const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency; // Map value to Timer ranges 0..max_duty_cycle => 0..TC // Setup Timer for this pin - ulValue = ulValue * TC; + ulValue = ulValue * TC ; pwm_counter_vals[channel] = ulValue / _max_pwm_value; // enable counter if (channelToAB[channel]) @@ -297,8 +327,8 @@ void TC8_Handler() // - BLDC motor - 3PWM setting // - hardware specific void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + 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 _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -314,8 +344,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int //- Stepper driver - 2PWM setting // - hardware specific void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + 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 _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -329,8 +359,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // - 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 || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + 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 _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -373,19 +403,4 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ } -// 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){ - return -1; -} - -// 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){ - return; -} - - #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 4a2360d9..6549de07 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,32 +1,13 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328 - -#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 - -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - -#elif defined(ESP_H) // or esp32 - -#elif defined(_STM32_DEF_) // or stm32 - -#elif defined(_SAMD21_) // samd21 - -#elif defined(_SAMD51_) // samd51 - -#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 - -#elif defined(TARGET_RP2040) - -#else - // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); + _UNUSED(pinA); + _UNUSED(pinB); return; } @@ -34,7 +15,11 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +__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; } @@ -43,14 +28,27 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +__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; } // 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){ +__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; } @@ -58,7 +56,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // 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){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] analogWrite(pinA, 255.0*dc_a); analogWrite(pinB, 255.0*dc_b); @@ -67,7 +65,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // 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){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] analogWrite(pinA, 255.0*dc_a); analogWrite(pinB, 255.0*dc_b); @@ -77,7 +75,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // 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){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] analogWrite(pin1A, 255.0*dc_1a); analogWrite(pin1B, 255.0*dc_1b); @@ -89,9 +87,17 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // 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){ +__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){ + _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; } - -#endif diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 8ec19643..5f70db4f 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -74,18 +74,4 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in analogWrite(pin2B, 255.0*dc_2b); } - -// 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){ - return -1; -} - -// 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){ - return; -} #endif \ No newline at end of file From 9af38faec14e1ae8e0bbd46243a03f7ada6e6cc3 Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 15 May 2021 11:13:58 -0600 Subject: [PATCH 006/103] SAMDCurrentSenseADCDMA class --- src/current_sense/LowSideCurrentSense.cpp | 4 +- src/current_sense/LowSideCurrentSense.h | 2 +- .../hardware_specific/samd21_mcu.cpp | 157 ++++++------------ .../hardware_specific/samd21_mcu.h | 76 ++++----- 4 files changed, 91 insertions(+), 148 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 0c12e528..7f6b3ceb 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,8 +55,8 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - adc._start3PinsDMA(); + adc.readResults(current.a, current.b, current.c); + adc.startADCScan(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 2a898bfc..379e441c 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -53,7 +53,7 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - SAMDCurrentSensceADC adc; + SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index c95bb754..f511ec1d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,9 +1,10 @@ #include "samd21_mcu.h" + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -void adc_stop_with_DMA(void); -void adc_start_with_DMA(void); +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); /** * @brief ADC sync wait @@ -16,91 +17,66 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) -: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) { - ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); - this->pinA = pinA; - this->pinB = pinB; - this->pinC = pinC; + countsToVolts = ( voltageAREF / maxCountsADC ); } -void SAMDCurrentSensceADC::init() -{ - _configure3PinsDMA(); - _start3PinsDMA(); //s +void SAMDCurrentSenseADCDMA::init(){ + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() } -void SAMDCurrentSensceADC::_start3PinsDMA() -{ - adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); - adc_start_with_DMA(); +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); } -void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) -{ + +void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ while(ADC->CTRLA.bit.ENABLE) ; - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[adcA] * ADC_CONV_; - b = adcBuffer[adcB] * ADC_CONV_; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + a = adcBuffer[ainA] * countsToVolts; + b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[adcC] * ADC_CONV_; + c = adcBuffer[ainC] * countsToVolts; } } -// function reading an ADC value and returning the read voltage -void SAMDCurrentSensceADC::_configure3PinsDMA(){ +void SAMDCurrentSenseADCDMA::initPins(){ - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - - pinMode(42, INPUT); - ADC_FirstPin = min(adcA, adcB); - ADC_LastPin = max(adcA, adcB); + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); if( _isset(pinC) ) { pinMode(pinC, INPUT); - ADC_FirstPin = min(ADC_FirstPin, adcC); - ADC_LastPin = max(ADC_LastPin, adcC); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); } - ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout - BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; - // ADC and DMA - adc_init(); - dma_init(); } - - - - -/** - * @brief Initialize ADC - * @retval void - */ -void SAMDCurrentSensceADC::adc_init(){ +void SAMDCurrentSenseADCDMA::initADC(){ analogRead(pinA); // do some pin init pinPeripheral() analogRead(pinB); // do some pin init pinPeripheral() @@ -155,9 +131,9 @@ void SAMDCurrentSensceADC::adc_init(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -169,12 +145,9 @@ void SAMDCurrentSensceADC::adc_init(){ ADCsync(); } +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -/** - * @brief dma_init - * @retval void - */ -void SAMDCurrentSensceADC::dma_init() { +void SAMDCurrentSenseADCDMA::initDMA() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -184,68 +157,46 @@ void SAMDCurrentSensceADC::dma_init() { DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); } -/** - * @brief adc_dma - * @retval void - */ -void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { - uint32_t temp_CHCTRLB_reg; - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); - temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | - DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; descriptor.btcnt = hwords; descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); // start channel - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } - - -/** - * @brief adc_stop_with_DMA - * @retval void - */ -void adc_stop_with_DMA(void) -{ +void adcStopWithDMA(void){ ADC->CTRLA.bit.ENABLE = 0x00; - // SerialUSB.println("DMA stopped!"); } -/** - * @brief adc_start_with_DMA - * @retval void - */ -void adc_start_with_DMA(void) -{ - // SerialUSB.println("strating DMA..."); - // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; +void adcStartWithDMA(void){ ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } -/** - * @brief DMAC_Handler - * @retval void - */ + void DMAC_Handler() { uint8_t active_channel; active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adc_stop_with_DMA(); + adcStopWithDMA(); DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index fb5d319a..c410b522 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,60 +1,52 @@ #include "../hardware_api.h" - -class SAMDCurrentSensceADC + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + +class SAMDCurrentSenseADCDMA { public: - SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void init(); - // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless - uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout - uint32_t ADC_FirstPin; // PA04 - uint32_t ADC_LastPin; // PA06 - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA = A4; - uint32_t pinB = A5; - uint32_t pinC = 8; - - float _ADC_VOLTAGE; - float _ADC_RESOLUTION; - float ADC_CONV_; + void startADCScan(); + void readResults(float & a, float & b, float & c); - void _start3PinsDMA(); - void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); - // function reading an ADC value and returning the read voltage - void _configure3PinsDMA(); +private: + void adcToDMATransfer(void *rxdata, size_t hwords); + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; - - uint32_t ADC_DMA_chnl = 3; // DMA channel - - - /** - * @brief Initialize ADC - * @retval void - */ - void adc_init(); + uint16_t adcBuffer[20]; - /** - * @brief dma_init - * @retval void - */ - void dma_init(); + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel - /** - * @brief adc_dma - * @retval void - */ - void adc_dma(void *rxdata, size_t hwords); + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); }; From 7698c0ebcbd67d545c677d5db0087cfd4d1fc119 Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 08:32:25 -0600 Subject: [PATCH 007/103] FEAT ADC-DMA fix deadlock --- src/current_sense/LowSideCurrentSense.cpp | 7 +++++-- src/current_sense/LowSideCurrentSense.h | 3 ++- .../hardware_specific/samd21_mcu.cpp | 18 +++++++++++++----- .../hardware_specific/samd21_mcu.h | 6 +++--- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 7f6b3ceb..46110c34 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,10 +55,13 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc.readResults(current.a, current.b, current.c); + if(adc.readResults(current.a, current.b, current.c)) + { + oldCurrent = current; + } adc.startADCScan(); - return current; + return oldCurrent; } // Function synchronizing current sense with motor driver. // for in-line sensig no such thing is necessary diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 379e441c..461f3e35 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -52,7 +52,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ia; //!< zero current A voltage value (center of the adc reading) double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - + + PhaseCurrent_s oldCurrent; SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f511ec1d..e9f43ef7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -40,17 +40,19 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ - while(ADC->CTRLA.bit.ENABLE) ; +bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; a = adcBuffer[ainA] * countsToVolts; b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; c = adcBuffer[ainC] * countsToVolts; } + return true; } void SAMDCurrentSenseADCDMA::initPins(){ @@ -61,11 +63,11 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); @@ -158,7 +160,7 @@ void SAMDCurrentSenseADCDMA::initDMA() { } -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; @@ -183,13 +185,19 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { void adcStopWithDMA(void){ + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; + } void adcStartWithDMA(void){ + ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); } void DMAC_Handler() { diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c410b522..1c1603b9 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,6 +1,6 @@ #include "../hardware_api.h" - +#include typedef struct { uint16_t btctrl; uint16_t btcnt; @@ -18,11 +18,11 @@ class SAMDCurrentSenseADCDMA void init(); void startADCScan(); - void readResults(float & a, float & b, float & c); + bool readResults(float & a, float & b, float & c); private: - void adcToDMATransfer(void *rxdata, size_t hwords); + void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); From a0d17a097aadf6696697ef7428e59cc5dd2aaf97 Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 11:12:36 -0600 Subject: [PATCH 008/103] Samd support add support for serial insterface other than Serial I use -D SIMPLEFOC_SAMD_DEBUG_SERIAL=SERIAL_PORT_MONITOR --- src/drivers/hardware_specific/samd21_mcu.cpp | 34 ++-- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +-- src/drivers/hardware_specific/samd_mcu.cpp | 162 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 5 +- 4 files changed, 112 insertions(+), 109 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ 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 - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 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 - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,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 - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("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 @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + 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("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" From db7d8653f498b54bd223034676d2c8a388833d62 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 19:19:06 -0600 Subject: [PATCH 009/103] LowSideCurrentSense: first (truly) successful DMA+ADC readings, 1.65-centered readings with the drv8305, just as intented --- src/current_sense/LowSideCurrentSense.cpp | 177 ++++++++++++ src/current_sense/LowSideCurrentSense.h | 57 ++++ src/current_sense/hardware_api.h | 19 -- .../hardware_specific/samd21_mcu.cpp | 266 ++++++++++++++++++ 4 files changed, 500 insertions(+), 19 deletions(-) create mode 100644 src/current_sense/LowSideCurrentSense.cpp create mode 100644 src/current_sense/LowSideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp new file mode 100644 index 00000000..f9647f7b --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -0,0 +1,177 @@ +#include "LowSideCurrentSense.h" +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Inline sensor init function +void LowSideCurrentSense::init(){ + // configure ADC variables + _configure3PinsDMA(pinA,pinB,pinC); + _start3PinsDMA(); //start next acuisition + // calibrate zero offsets + // calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowSideCurrentSense::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 += _readADCVoltage(pinA); + offset_ib += _readADCVoltage(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps + // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps + // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + _start3PinsDMA(); + + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowSideCurrentSense::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 LowSideCurrentSense::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.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // 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.7 ){ // 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.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // 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.7 ){ // 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.0; + } + 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/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h new file mode 100644 index 00000000..48c6dc80 --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.h @@ -0,0 +1,57 @@ +#ifndef LOW_SIDE_CS_LIB_H +#define LOW_SIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "hardware_api.h" + +class LowSideCurrentSense: public CurrentSense{ + public: + /** + LowSideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 4c4c1086..9ca93ae7 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,23 +20,4 @@ float _readADCVoltageInline(const int pinA); */ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); -/** - * 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 = NOT_SET); -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - the arduino pin to be read (it has to be ADC pin) - */ -float _readADCVoltageLowSide(const int pinA); - -/** - * function syncing the Driver with the ADC for the LowSide Sensing - */ -void _driverSyncLowSide(); #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..fb0dbeaf --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,266 @@ + +#include "../hardware_api.h" + +// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless +static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout +static uint32_t ADC_FirstPin; // PA04 +static uint32_t ADC_LastPin; // PA06 +static uint32_t BufferSize = 0; + +uint16_t adcBuffer[20]; + + +static uint32_t ADC_pinA = A4; +static uint32_t ADC_pinB = A5; +static uint32_t ADC_pinC = 8; + + +#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val +#define _ADC_RESOLUTION (1 << 12) +#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) + +void adc_dma(void *rxdata, size_t hwords); +void adc_init(); +void adc_start_with_DMA(); +void dma_init(); + +void _start3PinsDMA() +{ + adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); + adc_start_with_DMA(); +} +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +{ + while(ADC->CTRLA.bit.ENABLE) ; + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[adcA] * ADC_CONV_; + b = adcBuffer[adcB] * ADC_CONV_; + if(_isset(pinC)) + { + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[adcC] * ADC_CONV_; + } +} + +// function reading an ADC value and returning the read voltage +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ + + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + + pinMode(42, INPUT); + ADC_FirstPin = min(adcA, adcB); + ADC_LastPin = max(adcA, adcB); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) + { + pinMode(pinC, INPUT); + ADC_FirstPin = min(ADC_FirstPin, adcC); + ADC_LastPin = max(ADC_LastPin, adcC); + } + + ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout + BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + + ADC_pinA = pinA; + ADC_pinB = pinB; + ADC_pinC = pinC; + + // ADC and DMA + adc_init(); + dma_init(); +} + + + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) + +static uint32_t ADC_DMA_chnl = 3; // DMA channel + + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +/** + * @brief Initialize ADC + * @retval void + */ +void adc_init(){ + + auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //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; + ADCsync(); // ref 31.6.16 + + a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + SerialUSB.println("--------------"); + SerialUSB.println(a); + SerialUSB.println(b); + SerialUSB.println(c); + SerialUSB.println("--------------"); +/* +Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan +This register gives the number of input sources included in the pin scan. The number of input sources included is +INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + +INPUTOFFSET + INPUTSCAN. +The range of the scan mode must not exceed the number of input channels available on the device. +Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection +These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If +the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit +group in the SamplingControl register must be written. +Table 32-14. Positive Mux Input Selection +MUXPOS[4:0] Group configuration Description +0x00 PIN0 ADC AIN0 pin +0x01 PIN1 ADC AIN1 pin +0x02 PIN2 ADC AIN2 pin +0x03 PIN3 ADC AIN3 pin +0x04 PIN4 ADC AIN4 pin +0x05 PIN5 ADC AIN5 pin +0x06 PIN6 ADC AIN6 pin +0x07 PIN7 ADC AIN7 pin +0x08 PIN8 ADC AIN8 pin +0x09 PIN9 ADC AIN9 pin +0x0A PIN10 ADC AIN10 pin +0x0B PIN11 ADC AIN11 pin +0x0C PIN12 ADC AIN12 pin +0x0D PIN13 ADC AIN13 pin +0x0E PIN14 ADC AIN14 pin +0x0F PIN15 ADC AIN15 pin +0x10 PIN16 ADC AIN16 pin +0x11 PIN17 ADC AIN17 pin +0x12 PIN18 ADC AIN18 pin +0x13 PIN19 ADC AIN19 pin +0x14-0x17 Reserved +0x18 TEMP Temperature reference +0x19 BANDGAP Bandgap voltage +0x1A SCALEDCOREVCC 1/4 scaled core supply +0x1B SCALEDIOVCC 1/4 scaled I/O supply +0x1C DAC DAC output +0x1D-0x1F Reserved +*/ + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + + +/** + * @brief dma_init + * @retval void + */ +void dma_init() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + +/** + * @brief adc_dma + * @retval void + */ +void adc_dma(void *rxdata, size_t hwords) { + uint32_t temp_CHCTRLB_reg; + + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); + temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | +DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + +/** + * @brief adc_stop_with_DMA + * @retval void + */ +void adc_stop_with_DMA(void) +{ + ADC->CTRLA.bit.ENABLE = 0x00; + // SerialUSB.println("DMA stopped!"); +} + +/** + * @brief adc_start_with_DMA + * @retval void + */ +void adc_start_with_DMA(void) +{ + // SerialUSB.println("strating DMA..."); + + + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADC->SWTRIG.bit.FLUSH = 1; + ADC->CTRLA.bit.ENABLE = 0x01; +} + + +/** + * @brief DMAC_Handler + * @retval void + */ +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adc_stop_with_DMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; +} From 7d3c011770301f5a27e00ddb94e42df4749cdb0e Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 20:26:26 -0600 Subject: [PATCH 010/103] wrapped samd21 adc/dma implementation in an ugly class --- src/SimpleFOC.h | 1 + src/current_sense/LowSideCurrentSense.cpp | 13 +- src/current_sense/LowSideCurrentSense.h | 3 + .../hardware_specific/samd21_mcu.cpp | 208 ++++++++---------- .../hardware_specific/samd21_mcu.h | 60 +++++ 5 files changed, 168 insertions(+), 117 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index f9647f7b..0c12e528 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -5,7 +5,9 @@ // - phA - A phase adc pin // - phB - B phase adc pin // - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): +adc(_pinA, _pinB, _pinC) +{ pinA = _pinA; pinB = _pinB; pinC = _pinC; @@ -21,9 +23,7 @@ LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int // Inline sensor init function void LowSideCurrentSense::init(){ - // configure ADC variables - _configure3PinsDMA(pinA,pinB,pinC); - _start3PinsDMA(); //start next acuisition + adc.init(); // calibrate zero offsets // calibrateOffsets(); } @@ -54,8 +54,9 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - _start3PinsDMA(); + + adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + adc._start3PinsDMA(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 48c6dc80..2a898bfc 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -6,6 +6,7 @@ #include "../common/time_utils.h" #include "../common/base_classes/CurrentSense.h" #include "hardware_api.h" +#include "hardware_specific/samd21_mcu.h" class LowSideCurrentSense: public CurrentSense{ public: @@ -52,6 +53,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) + SAMDCurrentSensceADC adc; + }; #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index fb0dbeaf..c95bb754 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,35 +1,57 @@ -#include "../hardware_api.h" +#include "samd21_mcu.h" -// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout -static uint32_t ADC_FirstPin; // PA04 -static uint32_t ADC_LastPin; // PA06 -static uint32_t BufferSize = 0; -uint16_t adcBuffer[20]; +void adc_stop_with_DMA(void); +void adc_start_with_DMA(void); +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -static uint32_t ADC_pinA = A4; -static uint32_t ADC_pinB = A5; -static uint32_t ADC_pinC = 8; -#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val -#define _ADC_RESOLUTION (1 << 12) -#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) +SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) +: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) +{ + ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; +} + +void SAMDCurrentSensceADC::init() +{ + _configure3PinsDMA(); + _start3PinsDMA(); //s +} -void adc_dma(void *rxdata, size_t hwords); -void adc_init(); -void adc_start_with_DMA(); -void dma_init(); -void _start3PinsDMA() +void SAMDCurrentSensceADC::_start3PinsDMA() { adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); adc_start_with_DMA(); } -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) { while(ADC->CTRLA.bit.ENABLE) ; uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; @@ -44,7 +66,7 @@ void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, floa } // function reading an ADC value and returning the read voltage -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ +void SAMDCurrentSensceADC::_configure3PinsDMA(){ uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; @@ -65,10 +87,6 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; - ADC_pinA = pinA; - ADC_pinB = pinB; - ADC_pinC = pinC; - // ADC and DMA adc_init(); dma_init(); @@ -76,41 +94,17 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ -// ADC DMA sequential free running (6) with Interrupts ///////////////// - -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) - -static uint32_t ADC_DMA_chnl = 3; // DMA channel -/** - * @brief ADC sync wait - * @retval void - */ -static __inline__ void ADCsync() __attribute__((always_inline, unused)); -static void ADCsync() { - while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free -} - /** * @brief Initialize ADC * @retval void */ -void adc_init(){ +void SAMDCurrentSensceADC::adc_init(){ - auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); @@ -121,54 +115,46 @@ void adc_init(){ // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 - a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - c = analogRead(ADC_pinC); // do some pin init pinPeripheral() - SerialUSB.println("--------------"); - SerialUSB.println(a); - SerialUSB.println(b); - SerialUSB.println(c); - SerialUSB.println("--------------"); -/* -Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan -This register gives the number of input sources included in the pin scan. The number of input sources included is -INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + -INPUTOFFSET + INPUTSCAN. -The range of the scan mode must not exceed the number of input channels available on the device. -Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection -These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If -the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit -group in the SamplingControl register must be written. -Table 32-14. Positive Mux Input Selection -MUXPOS[4:0] Group configuration Description -0x00 PIN0 ADC AIN0 pin -0x01 PIN1 ADC AIN1 pin -0x02 PIN2 ADC AIN2 pin -0x03 PIN3 ADC AIN3 pin -0x04 PIN4 ADC AIN4 pin -0x05 PIN5 ADC AIN5 pin -0x06 PIN6 ADC AIN6 pin -0x07 PIN7 ADC AIN7 pin -0x08 PIN8 ADC AIN8 pin -0x09 PIN9 ADC AIN9 pin -0x0A PIN10 ADC AIN10 pin -0x0B PIN11 ADC AIN11 pin -0x0C PIN12 ADC AIN12 pin -0x0D PIN13 ADC AIN13 pin -0x0E PIN14 ADC AIN14 pin -0x0F PIN15 ADC AIN15 pin -0x10 PIN16 ADC AIN16 pin -0x11 PIN17 ADC AIN17 pin -0x12 PIN18 ADC AIN18 pin -0x13 PIN19 ADC AIN19 pin -0x14-0x17 Reserved -0x18 TEMP Temperature reference -0x19 BANDGAP Bandgap voltage -0x1A SCALEDCOREVCC 1/4 scaled core supply -0x1B SCALEDIOVCC 1/4 scaled I/O supply -0x1C DAC DAC output -0x1D-0x1F Reserved -*/ + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; ADCsync(); ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] @@ -176,7 +162,8 @@ MUXPOS[4:0] Group configuration Description ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; ADCsync(); @@ -187,7 +174,7 @@ MUXPOS[4:0] Group configuration Description * @brief dma_init * @retval void */ -void dma_init() { +void SAMDCurrentSensceADC::dma_init() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -201,7 +188,7 @@ void dma_init() { * @brief adc_dma * @retval void */ -void adc_dma(void *rxdata, size_t hwords) { +void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { uint32_t temp_CHCTRLB_reg; DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); @@ -209,7 +196,7 @@ void adc_dma(void *rxdata, size_t hwords) { DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | -DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -224,6 +211,9 @@ DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } + + + /** * @brief adc_stop_with_DMA * @retval void @@ -241,16 +231,12 @@ void adc_stop_with_DMA(void) void adc_start_with_DMA(void) { // SerialUSB.println("strating DMA..."); - - - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } - - /** * @brief DMAC_Handler * @retval void @@ -263,4 +249,4 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..fb5d319a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,60 @@ + +#include "../hardware_api.h" + + +class SAMDCurrentSensceADC +{ + +public: + + SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + + void init(); + // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout + uint32_t ADC_FirstPin; // PA04 + uint32_t ADC_LastPin; // PA06 + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA = A4; + uint32_t pinB = A5; + uint32_t pinC = 8; + + float _ADC_VOLTAGE; + float _ADC_RESOLUTION; + float ADC_CONV_; + + void _start3PinsDMA(); + void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); + // function reading an ADC value and returning the read voltage + void _configure3PinsDMA(); + + + + + uint32_t ADC_DMA_chnl = 3; // DMA channel + + + /** + * @brief Initialize ADC + * @retval void + */ + void adc_init(); + + + /** + * @brief dma_init + * @retval void + */ + void dma_init(); + + /** + * @brief adc_dma + * @retval void + */ + void adc_dma(void *rxdata, size_t hwords); + +}; From a5c7528c102dbea2f3b64206592078a57aff146a Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 15 May 2021 11:13:58 -0600 Subject: [PATCH 011/103] SAMDCurrentSenseADCDMA class --- src/current_sense/LowSideCurrentSense.cpp | 4 +- src/current_sense/LowSideCurrentSense.h | 2 +- .../hardware_specific/samd21_mcu.cpp | 157 ++++++------------ .../hardware_specific/samd21_mcu.h | 76 ++++----- 4 files changed, 91 insertions(+), 148 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 0c12e528..7f6b3ceb 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,8 +55,8 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - adc._start3PinsDMA(); + adc.readResults(current.a, current.b, current.c); + adc.startADCScan(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 2a898bfc..379e441c 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -53,7 +53,7 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - SAMDCurrentSensceADC adc; + SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index c95bb754..f511ec1d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,9 +1,10 @@ #include "samd21_mcu.h" + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -void adc_stop_with_DMA(void); -void adc_start_with_DMA(void); +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); /** * @brief ADC sync wait @@ -16,91 +17,66 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) -: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) { - ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); - this->pinA = pinA; - this->pinB = pinB; - this->pinC = pinC; + countsToVolts = ( voltageAREF / maxCountsADC ); } -void SAMDCurrentSensceADC::init() -{ - _configure3PinsDMA(); - _start3PinsDMA(); //s +void SAMDCurrentSenseADCDMA::init(){ + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() } -void SAMDCurrentSensceADC::_start3PinsDMA() -{ - adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); - adc_start_with_DMA(); +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); } -void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) -{ + +void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ while(ADC->CTRLA.bit.ENABLE) ; - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[adcA] * ADC_CONV_; - b = adcBuffer[adcB] * ADC_CONV_; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + a = adcBuffer[ainA] * countsToVolts; + b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[adcC] * ADC_CONV_; + c = adcBuffer[ainC] * countsToVolts; } } -// function reading an ADC value and returning the read voltage -void SAMDCurrentSensceADC::_configure3PinsDMA(){ +void SAMDCurrentSenseADCDMA::initPins(){ - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - - pinMode(42, INPUT); - ADC_FirstPin = min(adcA, adcB); - ADC_LastPin = max(adcA, adcB); + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); if( _isset(pinC) ) { pinMode(pinC, INPUT); - ADC_FirstPin = min(ADC_FirstPin, adcC); - ADC_LastPin = max(ADC_LastPin, adcC); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); } - ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout - BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; - // ADC and DMA - adc_init(); - dma_init(); } - - - - -/** - * @brief Initialize ADC - * @retval void - */ -void SAMDCurrentSensceADC::adc_init(){ +void SAMDCurrentSenseADCDMA::initADC(){ analogRead(pinA); // do some pin init pinPeripheral() analogRead(pinB); // do some pin init pinPeripheral() @@ -155,9 +131,9 @@ void SAMDCurrentSensceADC::adc_init(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -169,12 +145,9 @@ void SAMDCurrentSensceADC::adc_init(){ ADCsync(); } +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -/** - * @brief dma_init - * @retval void - */ -void SAMDCurrentSensceADC::dma_init() { +void SAMDCurrentSenseADCDMA::initDMA() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -184,68 +157,46 @@ void SAMDCurrentSensceADC::dma_init() { DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); } -/** - * @brief adc_dma - * @retval void - */ -void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { - uint32_t temp_CHCTRLB_reg; - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); - temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | - DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; descriptor.btcnt = hwords; descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); // start channel - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } - - -/** - * @brief adc_stop_with_DMA - * @retval void - */ -void adc_stop_with_DMA(void) -{ +void adcStopWithDMA(void){ ADC->CTRLA.bit.ENABLE = 0x00; - // SerialUSB.println("DMA stopped!"); } -/** - * @brief adc_start_with_DMA - * @retval void - */ -void adc_start_with_DMA(void) -{ - // SerialUSB.println("strating DMA..."); - // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; +void adcStartWithDMA(void){ ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } -/** - * @brief DMAC_Handler - * @retval void - */ + void DMAC_Handler() { uint8_t active_channel; active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adc_stop_with_DMA(); + adcStopWithDMA(); DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index fb5d319a..c410b522 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,60 +1,52 @@ #include "../hardware_api.h" - -class SAMDCurrentSensceADC + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + +class SAMDCurrentSenseADCDMA { public: - SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void init(); - // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless - uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout - uint32_t ADC_FirstPin; // PA04 - uint32_t ADC_LastPin; // PA06 - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA = A4; - uint32_t pinB = A5; - uint32_t pinC = 8; - - float _ADC_VOLTAGE; - float _ADC_RESOLUTION; - float ADC_CONV_; + void startADCScan(); + void readResults(float & a, float & b, float & c); - void _start3PinsDMA(); - void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); - // function reading an ADC value and returning the read voltage - void _configure3PinsDMA(); +private: + void adcToDMATransfer(void *rxdata, size_t hwords); + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; - - uint32_t ADC_DMA_chnl = 3; // DMA channel - - - /** - * @brief Initialize ADC - * @retval void - */ - void adc_init(); + uint16_t adcBuffer[20]; - /** - * @brief dma_init - * @retval void - */ - void dma_init(); + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel - /** - * @brief adc_dma - * @retval void - */ - void adc_dma(void *rxdata, size_t hwords); + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); }; From 7e802f682927c54b2490beadb9057295b8d7a12c Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 08:32:25 -0600 Subject: [PATCH 012/103] FEAT ADC-DMA fix deadlock --- src/current_sense/LowSideCurrentSense.cpp | 7 +++++-- src/current_sense/LowSideCurrentSense.h | 3 ++- .../hardware_specific/samd21_mcu.cpp | 18 +++++++++++++----- .../hardware_specific/samd21_mcu.h | 6 +++--- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 7f6b3ceb..46110c34 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,10 +55,13 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc.readResults(current.a, current.b, current.c); + if(adc.readResults(current.a, current.b, current.c)) + { + oldCurrent = current; + } adc.startADCScan(); - return current; + return oldCurrent; } // Function synchronizing current sense with motor driver. // for in-line sensig no such thing is necessary diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 379e441c..461f3e35 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -52,7 +52,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ia; //!< zero current A voltage value (center of the adc reading) double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - + + PhaseCurrent_s oldCurrent; SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f511ec1d..e9f43ef7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -40,17 +40,19 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ - while(ADC->CTRLA.bit.ENABLE) ; +bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; a = adcBuffer[ainA] * countsToVolts; b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; c = adcBuffer[ainC] * countsToVolts; } + return true; } void SAMDCurrentSenseADCDMA::initPins(){ @@ -61,11 +63,11 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); @@ -158,7 +160,7 @@ void SAMDCurrentSenseADCDMA::initDMA() { } -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; @@ -183,13 +185,19 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { void adcStopWithDMA(void){ + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; + } void adcStartWithDMA(void){ + ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); } void DMAC_Handler() { diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c410b522..1c1603b9 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,6 +1,6 @@ #include "../hardware_api.h" - +#include typedef struct { uint16_t btctrl; uint16_t btcnt; @@ -18,11 +18,11 @@ class SAMDCurrentSenseADCDMA void init(); void startADCScan(); - void readResults(float & a, float & b, float & c); + bool readResults(float & a, float & b, float & c); private: - void adcToDMATransfer(void *rxdata, size_t hwords); + void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); From bb7d29c655cd96bfdaf72e9103c10c5f41d87aff Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 12:10:46 -0600 Subject: [PATCH 013/103] FEAT add _startADC3PinConversionLowSide() API --- src/SimpleFOC.h | 2 +- src/current_sense/LowSideCurrentSense.cpp | 181 ------------------ src/current_sense/LowSideCurrentSense.h | 61 ------ src/current_sense/LowsideCurrentSense.cpp | 2 + src/current_sense/hardware_api.h | 24 ++- .../hardware_specific/esp32_mcu.cpp | 3 + .../hardware_specific/samd21_mcu.cpp | 115 +++++++++-- .../hardware_specific/samd21_mcu.h | 23 ++- 8 files changed, 147 insertions(+), 264 deletions(-) delete mode 100644 src/current_sense/LowSideCurrentSense.cpp delete mode 100644 src/current_sense/LowSideCurrentSense.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 02e215b9..4f9f1108 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,7 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" -#include "current_sense/LowSideCurrentSense.h" +#include "current_sense/LowsideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp deleted file mode 100644 index 46110c34..00000000 --- a/src/current_sense/LowSideCurrentSense.cpp +++ /dev/null @@ -1,181 +0,0 @@ -#include "LowSideCurrentSense.h" -// InlineCurrentSensor constructor -// - shunt_resistor - shunt resistor value -// - gain - current-sense op-amp gain -// - phA - A phase adc pin -// - phB - B phase adc pin -// - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): -adc(_pinA, _pinB, _pinC) -{ - pinA = _pinA; - pinB = _pinB; - pinC = _pinC; - - shunt_resistor = _shunt_resistor; - amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps - // gains for each phase - gain_a = volts_to_amps_ratio; - gain_b = volts_to_amps_ratio; - gain_c = volts_to_amps_ratio; -} - -// Inline sensor init function -void LowSideCurrentSense::init(){ - adc.init(); - // calibrate zero offsets - // calibrateOffsets(); -} -// Function finding zero offsets of the ADC -void LowSideCurrentSense::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 += _readADCVoltage(pinA); - offset_ib += _readADCVoltage(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); - _delay(1); - } - // calculate the mean offsets - offset_ia = offset_ia / calibration_rounds; - offset_ib = offset_ib / calibration_rounds; - if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; -} - -// read all three phase currents (if possible 2 or 3) -PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ - PhaseCurrent_s current; - // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps - // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - - if(adc.readResults(current.a, current.b, current.c)) - { - oldCurrent = current; - } - adc.startADCScan(); - - return oldCurrent; -} -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int LowSideCurrentSense::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 LowSideCurrentSense::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.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 - gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // 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.7 ){ // 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.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*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.5 ){ // should be ~2 - gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // 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.7 ){ // 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.0; - } - 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/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h deleted file mode 100644 index 461f3e35..00000000 --- a/src/current_sense/LowSideCurrentSense.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef LOW_SIDE_CS_LIB_H -#define LOW_SIDE_CS_LIB_H - -#include "Arduino.h" -#include "../common/foc_utils.h" -#include "../common/time_utils.h" -#include "../common/base_classes/CurrentSense.h" -#include "hardware_api.h" -#include "hardware_specific/samd21_mcu.h" - -class LowSideCurrentSense: public CurrentSense{ - public: - /** - LowSideCurrentSense class constructor - @param shunt_resistor shunt resistor value - @param gain current-sense op-amp gain - @param phA A phase adc pin - @param phB B phase adc pin - @param phC C phase adc pin (optional) - */ - LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); - - // CurrentSense interface implementing functions - void init() override; - PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; - - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - private: - - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - - // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio - - /** - * Function finding zero offsets of the ADC - */ - void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) - - PhaseCurrent_s oldCurrent; - SAMDCurrentSenseADCDMA adc; - -}; - -#endif \ No newline at end of file diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index abebf03d..e0df940a 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -34,6 +34,7 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ic= 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < 1000; i++) { + _startADC3PinConversionLowSide(); offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); @@ -48,6 +49,7 @@ void LowsideCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) 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 diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 8a743bf0..7b648cde 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,8 +20,26 @@ float _readADCVoltageInline(const int pinA); */ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); -void _start3PinsDMA(); -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC = NOT_SET); +/** + * 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 = NOT_SET); +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) + */ +float _readADCVoltageLowSide(const int pinA); + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide(); #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 7f84f9a9..3f7e83b3 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -30,6 +30,9 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } +void _startADC3PinConversionLowSide(){ + +} // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index e9f43ef7..5c5de779 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,5 +1,69 @@ #include "samd21_mcu.h" +#include "../hardware_api.h" + + +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) +{ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} +void _startADC3PinConversionLowSide() +{ + instance.startADCScan(); +} +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA) +{ + instance.readResults(a, b, c); + + if(pinA == _pinA) + return instance.toVolts(a); + if(pinA == _pinB) + return instance.toVolts(b); + if(pinA == _pinC) + return instance.toVolts(c); + + return NAN; +} + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide() +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); + instance.startADCScan(); + //TODO: hook with PWM interrupts +} + + + + + + + + + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless @@ -17,17 +81,27 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +{ + + return &instance; +} +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +{ +} - - -SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) -: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) { + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; + this->pinAREF = pinAREF; + this->channelDMA = channelDMA; + this->voltageAREF = voltageAREF; + this->maxCountsADC = 1 << adcBits; countsToVolts = ( voltageAREF / maxCountsADC ); -} -void SAMDCurrentSenseADCDMA::init(){ initPins(); initADC(); initDMA(); @@ -40,21 +114,26 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ +bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ if(ADC->CTRLA.bit.ENABLE) return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[ainA] * countsToVolts; - b = adcBuffer[ainB] * countsToVolts; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; if(_isset(pinC)) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[ainC] * countsToVolts; + c = adcBuffer[ainC]; } return true; } + +float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { + return counts * countsToVolts; +} + void SAMDCurrentSenseADCDMA::initPins(){ pinMode(pinAREF, INPUT); @@ -184,10 +263,23 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { } +int iii = 0; + void adcStopWithDMA(void){ ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; - + // ADCsync(); + // if(iii++ % 1000 == 0) + // { + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); + // } + + } void adcStartWithDMA(void){ @@ -208,4 +300,5 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + } \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index 1c1603b9..a01c72e3 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,5 +1,11 @@ +#ifndef CURRENT_SENSE_SAMD21_H +#define CURRENT_SENSE_SAMD21_H + +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif -#include "../hardware_api.h" #include typedef struct { uint16_t btctrl; @@ -9,17 +15,17 @@ uint32_t descaddr; } dmacdescriptor ; + class SAMDCurrentSenseADCDMA { public: - - SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); - - void init(); + static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); + SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void startADCScan(); - bool readResults(float & a, float & b, float & c); - + bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + float toVolts(uint16_t counts); private: void adcToDMATransfer(void *rxdata, uint32_t hwords); @@ -41,6 +47,7 @@ class SAMDCurrentSenseADCDMA uint32_t pinC; uint32_t pinAREF; uint32_t channelDMA; // DMA channel + bool freeRunning; float voltageAREF; float maxCountsADC; @@ -50,3 +57,5 @@ class SAMDCurrentSenseADCDMA dmacdescriptor descriptor __attribute__ ((aligned (16))); }; + +#endif \ No newline at end of file From dce977457ea735d9c18d702ff3187e2b7302c2e4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 15:09:43 +0200 Subject: [PATCH 014/103] remove oddities in MagneticSensorSPI - fix issue #82 --- src/sensors/MagneticSensorSPI.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b3d82dee..5ee9ecf9 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -75,11 +75,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif + // 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); // velocity calculation init @@ -170,35 +167,27 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); #if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word From e3bd3ac33366025c6d903c5b64edc58a7494bbd2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 22:37:07 +0200 Subject: [PATCH 015/103] sensor precision fix, part 1 --- src/common/base_classes/Sensor.cpp | 60 ++++++++++++++++++-------- src/common/base_classes/Sensor.h | 69 ++++++++++++++++++++++++++---- src/sensors/MagneticSensorI2C.cpp | 43 +------------------ src/sensors/MagneticSensorI2C.h | 14 +----- src/sensors/MagneticSensorSPI.cpp | 51 +--------------------- src/sensors/MagneticSensorSPI.h | 4 +- 6 files changed, 108 insertions(+), 133 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..93f4e7fe 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,31 +2,53 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** - * returns 0 if it does need search for absolute zero - * 0 - magnetic sensor (& encoder with index which is found) - * 1 - ecoder with index (with index not found yet) - */ -int Sensor::needsSearch(){ - return 0; + +float Sensor::getAngle() { + float val = getShaftAngle(); + angle_prev_ts = _micros(); + float d_angle = val - angle_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; + angle_prev = val; + return val; } - /** get current angular velocity (rad/s)*/ -float Sensor::getVelocity(){ + + /** get current angular velocity (rad/s)*/ +float Sensor::getVelocity() { // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - + float vel = (angle_prev - vel_angle_prev)/Ts; // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; + vel_angle_prev = angle_prev; + vel_angle_prev_ts = angle_prev_ts; return vel; +} + + + +float Sensor::getPosition(){ + return (float)full_rotations * _2PI + angle_prev; +} + + + +double Sensor::getPrecisePosition() { + return (double)full_rotations * (double)_2PI + (double)angle_prev; +} + + + +int32_t Sensor::getFullRotations() { + return full_rotations; +} + + + +int Sensor::needsSearch() { + return 0; // default false } \ No newline at end of file diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 3b219401..32ba7b25 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -1,6 +1,8 @@ #ifndef SENSOR_H #define SENSOR_H +#include + /** * Direction structure */ @@ -20,27 +22,76 @@ enum Pullup{ }; /** - * Sensor abstract class defintion - * Each sensor needs to have these functions implemented + * Sensor abstract class defintion + * + * This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have + * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the + * most basic common features so that a FOC driver can obtain the data it needs for operation. + * + * To implement your own sensors, create a sub-class of this class, and implement the getAngle() + * method. getAngle() returns a float value, in radians, representing the current shaft angle in the + * range 0 to 2*PI (one full turn). + * + * To function correctly, the sensor class getAngle() method has to be called sufficiently quickly. Normally, + * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly + * enough, both for correct motor and sensor operation. + * + * The Sensor base class provides an implementation of getVelocity(), and takes care of counting full + * revolutions in a precise way, but if you wish you can additionally override these methods to provide more + * optimal implementations for your hardware. + * */ class Sensor{ public: - - /** get current angle (rad) */ - virtual float getAngle()=0; - /** get current angular velocity (rad/s)*/ + /** + * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + */ + virtual float getAngle(); + /** + * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + * This method is pure virtual and must be implemented in subclasses. + * Calling this method directly does not update the base-class internal fields. + * Use getAngle() when calling from other code. + */ + virtual float getShaftAngle()=0; + /** + * Get current angular velocity (rad/s) + * Can be overridden in subclasses. Base implementation uses the values + * previously returned by getShaftAngle(). + */ virtual float getVelocity(); + /** + * Get current position (in rad) including full rotations and shaft angle + * Note that this value has limited precision as the number of rotations increases, + * as the limited precision of float can't capture the large angle of the full rotations + * and the small angle of the shaft angle at the same time. + */ + virtual float getPosition(); + + /** + * On architectures supporting it, this will return a double precision position value + */ + virtual double getPrecisePosition(); + + /** + * Get the number of full rotations + */ + virtual int32_t getFullRotations(); + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) */ virtual int needsSearch(); - private: + protected: // velocity calculation variables - float angle_prev=0; //!< angle in previous velocity calculation step - long velocity_calc_timestamp=0; //!< last velocity calculation timestamp + float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity + long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity + float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity + long vel_angle_prev_ts=0; //!< last velocity calculation timestamp + int32_t full_rotations=0; // full rotation tracking }; #endif diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index c9029f6e..7e2bc6c2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -64,54 +64,15 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // I2C communication begin wire->begin(); - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - // return the full angle +float MagneticSensorI2C::getShaftAngle(){ // (number of full rotations)*2PI + current sensor angle - return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; + return ( getRawCount() / (float)cpr) * _2PI ; } -// Shaft velocity calculation -float MagneticSensorI2C::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; -} // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 9966d6eb..f5119643 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -46,9 +46,7 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getShaftAngle() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); @@ -72,15 +70,7 @@ class MagneticSensorI2C: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //!begin(); // 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); - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - - // return the full angle - // (number of full rotations)*2PI + current sensor angle - return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; -} - -// Shaft velocity calculation -float MagneticSensorSPI::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; +float MagneticSensorSPI::getShaftAngle(){ + return (getRawCount() / (float)cpr) * _2PI; } - // function reading the raw counter of the magnetic sensor int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 77fcde4d..de5dea50 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -44,9 +44,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getShaftAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; From cb47fd0ae39cc1dcc0304c25e67c7427c47a1dda Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 22:40:23 +0200 Subject: [PATCH 016/103] mask out SAMD21 current sensing when compiling on other platforms --- src/current_sense/hardware_specific/samd21_mcu.cpp | 6 +++++- src/current_sense/hardware_specific/samd21_mcu.h | 6 ++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 5c5de779..8bb6d38c 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,3 +1,4 @@ +#ifdef _SAMD21_ #include "samd21_mcu.h" #include "../hardware_api.h" @@ -301,4 +302,7 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} \ No newline at end of file +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index a01c72e3..c0cec74a 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,3 +1,5 @@ +#ifdef _SAMD21_ + #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H @@ -58,4 +60,8 @@ class SAMDCurrentSenseADCDMA }; +#endif + + + #endif \ No newline at end of file From fef978238fbdce6b2a4c109d3a9305823eb9a6a4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 23:18:59 +0200 Subject: [PATCH 017/103] sensor precision fix, part 2, sensor changes --- src/sensors/Encoder.cpp | 17 ++++++++++++ src/sensors/Encoder.h | 5 ++++ src/sensors/HallSensor.cpp | 32 +++++++++++++++++++--- src/sensors/HallSensor.h | 8 ++++-- src/sensors/MagneticSensorAnalog.cpp | 40 +++------------------------- src/sensors/MagneticSensorAnalog.h | 15 +---------- src/sensors/MagneticSensorPWM.cpp | 37 +++---------------------- src/sensors/MagneticSensorPWM.h | 13 +-------- 8 files changed, 65 insertions(+), 102 deletions(-) diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..20506aee 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -102,8 +102,25 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getAngle(){ + return _2PI * (pulse_counter % (int)cpr); +} + +float Encoder::getPosition(){ return _2PI * (pulse_counter) / ((float)cpr); } +double Encoder::getPrecisePosition(){ + return _2PI * (pulse_counter) / ((double)cpr); +} +int32_t Encoder::getFullRotations(){ + return pulse_counter / (int)cpr; +} +float Encoder::getShaftAngle(){ + return getAngle(); +} + + + + /* Shaft velocity calculation function using mixed time and frequency measurement technique diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 7a0f7a40..679bad21 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -60,9 +60,14 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + float getPosition() override; + double getPrecisePosition() override; + int32_t getFullRotations() override; + /** * returns 0 if it does need search for absolute zero * 0 - encoder without index diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..063b3593 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,7 +96,7 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { Shaft angle calculation */ float HallSensor::getAngle() { - return ((electric_rotations * 6 + electric_sector) / cpr) * _2PI ; + return getShaftAngle(); } /* @@ -104,14 +104,39 @@ float HallSensor::getAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0); } } + + +float HallSensor::getShaftAngle() { + return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; +} + + +float HallSensor::getPosition() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; +} + + +double HallSensor::getPrecisePosition() { + return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; +} + + +int32_t HallSensor::getFullRotations() { + return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); +} + + + + + // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ @@ -149,4 +174,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index c836cc5d..28ab1578 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -50,13 +50,17 @@ class HallSensor: public Sensor{ // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups - float cpr;//!< HallSensor cpr number + int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + float getPosition() override; + double getPrecisePosition() override; + int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; @@ -77,7 +81,7 @@ class HallSensor: public Sensor{ Direction decodeDirection(int oldState, int newState); void updateState(); - volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel volatile int C_active; //!< current active states of C channel diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..7c343d63 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -23,47 +23,15 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getAngle(){ +float MagneticSensorAnalog::getShaftAngle(){ // raw data from the sensor - raw_count = getRawCount(); - - int delta = raw_count - raw_count_prev; - // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; -} - -// Shaft velocity calculation -float MagneticSensorAnalog::getVelocity(){ - // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle - return velocity; + raw_count = getRawCount(); + return ( (float) (raw_count) / (float)cpr) * _2PI; } // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 12f78c76..7212ad9a 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,10 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; - + float getShaftAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; @@ -48,16 +45,6 @@ class MagneticSensorAnalog: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //! (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; + return( (float) (raw_count) / (float)cpr) * _2PI; } -// get velocity (rad/s) -float MagneticSensorPWM::getVelocity(){ - return velocity; -} // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 32d6daee..645624d1 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -23,9 +23,7 @@ class MagneticSensorPWM: public Sensor{ int pinPWM; // get current angle (rad) - float getAngle() override; - // get current angular velocity (rad/s) - float getVelocity() override; + float getShaftAngle() override; // pwm handler void handlePWM(); @@ -50,15 +48,6 @@ class MagneticSensorPWM: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //! Date: Mon, 31 May 2021 00:41:38 +0200 Subject: [PATCH 018/103] sensor precision fix - refactored API --- src/common/base_classes/Sensor.cpp | 15 ++++--- src/common/base_classes/Sensor.h | 60 ++++++++++++++++++---------- src/sensors/Encoder.cpp | 13 +++--- src/sensors/Encoder.h | 6 +-- src/sensors/HallSensor.cpp | 21 +++++----- src/sensors/HallSensor.h | 4 +- src/sensors/MagneticSensorAnalog.cpp | 2 +- src/sensors/MagneticSensorAnalog.h | 2 +- src/sensors/MagneticSensorI2C.cpp | 2 +- src/sensors/MagneticSensorI2C.h | 2 +- src/sensors/MagneticSensorPWM.cpp | 2 +- src/sensors/MagneticSensorPWM.h | 2 +- src/sensors/MagneticSensorSPI.cpp | 2 +- src/sensors/MagneticSensorSPI.h | 10 +---- 14 files changed, 81 insertions(+), 62 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 93f4e7fe..4853a8aa 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -3,8 +3,9 @@ #include "../time_utils.h" -float Sensor::getAngle() { - float val = getShaftAngle(); + +float Sensor::updateSensor() { + float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation @@ -14,7 +15,6 @@ float Sensor::getAngle() { } - /** get current angular velocity (rad/s)*/ float Sensor::getVelocity() { // calculate sample time @@ -30,14 +30,19 @@ float Sensor::getVelocity() { } +float Sensor::getShaftAngle() { + return angle_prev; +} + + -float Sensor::getPosition(){ +float Sensor::getAngle(){ return (float)full_rotations * _2PI + angle_prev; } -double Sensor::getPrecisePosition() { +double Sensor::getPreciseAngle() { return (double)full_rotations * (double)_2PI + (double)angle_prev; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 32ba7b25..7f9aea7c 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -32,7 +32,7 @@ enum Pullup{ * method. getAngle() returns a float value, in radians, representing the current shaft angle in the * range 0 to 2*PI (one full turn). * - * To function correctly, the sensor class getAngle() method has to be called sufficiently quickly. Normally, + * To function correctly, the sensor class updateSensor() method has to be called sufficiently quickly. Normally, * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly * enough, both for correct motor and sensor operation. * @@ -43,17 +43,26 @@ enum Pullup{ */ class Sensor{ public: - /** - * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + /** + * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with + * the hardware. + */ + virtual float getShaftAngle(); + + /** + * Get current position (in rad) including full rotations and shaft angle + * Note that this value has limited precision as the number of rotations increases, + * because the limited precision of float can't capture the large angle of the full + * rotations and the small angle of the shaft angle at the same time. */ virtual float getAngle(); + /** - * Get current shaft angle, as a float in radians, in the range 0 to 2PI. - * This method is pure virtual and must be implemented in subclasses. - * Calling this method directly does not update the base-class internal fields. - * Use getAngle() when calling from other code. + * On architectures supporting it, this will return a double precision position value, + * which should have improved precision for large position values. */ - virtual float getShaftAngle()=0; + virtual double getPreciseAngle(); + /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values @@ -62,22 +71,23 @@ class Sensor{ virtual float getVelocity(); /** - * Get current position (in rad) including full rotations and shaft angle - * Note that this value has limited precision as the number of rotations increases, - * as the limited precision of float can't capture the large angle of the full rotations - * and the small angle of the shaft angle at the same time. - */ - virtual float getPosition(); - - /** - * On architectures supporting it, this will return a double precision position value + * Get the number of full rotations */ - virtual double getPrecisePosition(); + virtual int32_t getFullRotations(); /** - * Get the number of full rotations + * Updates the sensor values by reading the hardware sensor. + * Some implementations may work with interrupts, and not need this. + * The base implementation calls getSensorAngle(), and updates internal + * fields for angle, timestamp and full rotations. + * This method must be called frequently enough to guarantee that full + * rotations are not "missed" due to infrequent polling. + * Override in subclasses if alternative behaviours are required for your + * sensor hardware. + * + * Returns the current value of getShaftAngle() */ - virtual int32_t getFullRotations(); + virtual float updateSensor(); /** * returns 0 if it does need search for absolute zero @@ -86,6 +96,16 @@ class Sensor{ */ virtual int needsSearch(); protected: + /** + * Get current shaft angle from the sensor hardware, and + * return it as a float in radians, in the range 0 to 2PI. + * + * This method is pure virtual and must be implemented in subclasses. + * Calling this method directly does not update the base-class internal fields. + * Use updateSensor() when calling from outside code. + */ + virtual float getSensorAngle()=0; + // velocity calculation variables float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 20506aee..fdf1f065 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -101,23 +101,22 @@ void Encoder::handleIndex() { /* Shaft angle calculation */ -float Encoder::getAngle(){ +float Encoder::getSensorAngle(){ + return getShaftAngle(); +} +float Encoder::getShaftAngle(){ return _2PI * (pulse_counter % (int)cpr); } -float Encoder::getPosition(){ +float Encoder::getAngle(){ return _2PI * (pulse_counter) / ((float)cpr); } -double Encoder::getPrecisePosition(){ +double Encoder::getPreciseAngle(){ return _2PI * (pulse_counter) / ((double)cpr); } int32_t Encoder::getFullRotations(){ return pulse_counter / (int)cpr; } -float Encoder::getShaftAngle(){ - return getAngle(); -} - diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 679bad21..c3cab450 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -60,12 +60,12 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getSensorAngle() override; float getShaftAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getPosition() override; - double getPrecisePosition() override; + float getAngle() override; + double getPreciseAngle() override; int32_t getFullRotations() override; /** diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 063b3593..e563e376 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -92,11 +92,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } + + +float HallSensor::getSensorAngle() { + return getShaftAngle(); +} + + + /* Shaft angle calculation */ -float HallSensor::getAngle() { - return getShaftAngle(); +float HallSensor::getShaftAngle() { + return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; } /* @@ -114,17 +122,12 @@ float HallSensor::getVelocity(){ -float HallSensor::getShaftAngle() { - return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; -} - - -float HallSensor::getPosition() { +float HallSensor::getAngle() { return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } -double HallSensor::getPrecisePosition() { +double HallSensor::getPreciseAngle() { return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 28ab1578..ef991b77 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -54,12 +54,12 @@ class HallSensor: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getSensorAngle() override; float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getPosition() override; - double getPrecisePosition() override; + double getPreciseAngle() override; int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 7c343d63..04fc89ab 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -28,7 +28,7 @@ void MagneticSensorAnalog::init(){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getShaftAngle(){ +float MagneticSensorAnalog::getSensorAngle(){ // raw data from the sensor raw_count = getRawCount(); return ( (float) (raw_count) / (float)cpr) * _2PI; diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 7212ad9a..6f787b95 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,7 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 7e2bc6c2..dc3cac4c 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -68,7 +68,7 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getShaftAngle(){ +float MagneticSensorI2C::getSensorAngle(){ // (number of full rotations)*2PI + current sensor angle return ( getRawCount() / (float)cpr) * _2PI ; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index f5119643..29bebee4 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -46,7 +46,7 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index a97dc255..8320868f 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -30,7 +30,7 @@ void MagneticSensorPWM::init(){ } // get current angle (rad) -float MagneticSensorPWM::getShaftAngle(){ +float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor raw_count = getRawCount(); return( (float) (raw_count) / (float)cpr) * _2PI; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 645624d1..b42b23d6 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -23,7 +23,7 @@ class MagneticSensorPWM: public Sensor{ int pinPWM; // get current angle (rad) - float getShaftAngle() override; + float getSensorAngle() override; // pwm handler void handlePWM(); diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index e0f72cdb..89155b0a 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -78,7 +78,7 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getShaftAngle(){ +float MagneticSensorSPI::getSensorAngle(){ return (getRawCount() / (float)cpr) * _2PI; } diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index de5dea50..33aad3f3 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -44,7 +44,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; @@ -72,14 +72,6 @@ class MagneticSensorSPI: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //! Date: Mon, 31 May 2021 01:43:32 +0200 Subject: [PATCH 019/103] adapted motor code to new API, added comments for precision issues --- src/BLDCMotor.cpp | 56 ++++++++++++++++++++++-------- src/common/base_classes/Sensor.cpp | 2 +- src/common/base_classes/Sensor.h | 2 +- 3 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..d202aae2 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - shaft_angle = sensor->getAngle(); + shaft_angle = sensor->updateSensor(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -164,14 +164,14 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - float mid_angle = sensor->getAngle(); + float mid_angle = sensor->updateSensor(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0 ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - float end_angle = sensor->getAngle(); + float end_angle = sensor->updateSensor(); setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved @@ -201,7 +201,7 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->updateSensor(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -217,7 +217,8 @@ int BLDCMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroSearch() { - + // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision + // of float is sufficient. if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -229,7 +230,7 @@ int BLDCMotor::absoluteZeroSearch() { angleOpenloop(1.5*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->updateSensor(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -247,15 +248,25 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->updateSensor(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; + // shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; // electrical angle - need shaftAngle to be called first + // TODO sensor precision: this will have precision issues because the shaft_angle does... electrical_angle = electricalAngle(); switch (torque_controller) { @@ -300,14 +311,14 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { - // get angular velocity + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + // get angular velocity - sensor precision: this value is numerically precise. It is filtered by Velocity LPF, and downsamling (depending on sensor) shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; - // downsampling (optional) - if(motion_cnt++ < motion_downsample) return; - motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; @@ -320,11 +331,14 @@ void BLDCMotor::move(float new_target) { current_sp = target; // if current/foc_current torque control break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - // calculate the torque command + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ @@ -335,7 +349,7 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control @@ -348,13 +362,16 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor voltage.d = 0; @@ -572,6 +589,8 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // quick fix for strange cases (micros overflow + timestamp not defined) if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + // sensor precision: this calculation is numerically precise since we re-normalize + // the shaft-angle to the range 0-2PI // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes @@ -581,6 +600,8 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + // sensor precision: this calculation is numerically precise, since shaft_angle is + // in the range 0-2PI // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -603,6 +624,9 @@ float BLDCMotor::angleOpenloop(float target_angle){ // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) + // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point + // where small position changes are no longer captured by the precision of floats + // when the total position is large. if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; shaft_velocity = velocity_limit; @@ -616,6 +640,10 @@ float BLDCMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle + // TODO sensor precision: this calculation is not numerically precise. The angle is not constrained, + // and can grow large. It first gets multiplied by the number of pole-pairs (in _electrocalAngle()), + // and then gets normalized to 0-2PI (before it gets used in setPhaseVoltage()). At large angles, + // the precision can mean the range 0-2PI is not well represented at the end of that calculation. setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 4853a8aa..015dfca9 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -11,7 +11,7 @@ float Sensor::updateSensor() { // if overflow happened track it as full rotation if(abs(d_angle) > (0.8*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; angle_prev = val; - return val; + return getAngle(); } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 7f9aea7c..0f9a5efe 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -85,7 +85,7 @@ class Sensor{ * Override in subclasses if alternative behaviours are required for your * sensor hardware. * - * Returns the current value of getShaftAngle() + * Returns the same value as getAngle() as its result */ virtual float updateSensor(); From a5df2341754023d81a9742c207f54245ad1b6da4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 May 2021 01:53:47 +0200 Subject: [PATCH 020/103] add full rotations to velocity calculations --- src/common/base_classes/Sensor.cpp | 5 +++-- src/common/base_classes/Sensor.h | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 015dfca9..21f6797f 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,7 +2,7 @@ #include "../foc_utils.h" #include "../time_utils.h" - +// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 float Sensor::updateSensor() { float val = getSensorAngle(); @@ -22,9 +22,10 @@ float Sensor::getVelocity() { // quick fix for strange cases (micros overflow) if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // velocity calculation - float vel = (angle_prev - vel_angle_prev)/Ts; + float vel = ( (full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; // save variables for future pass vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; vel_angle_prev_ts = angle_prev_ts; return vel; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 0f9a5efe..c4c0dc6f 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -112,6 +112,7 @@ class Sensor{ float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity long vel_angle_prev_ts=0; //!< last velocity calculation timestamp int32_t full_rotations=0; // full rotation tracking + int32_t vel_full_rotations=0; }; #endif From c642756349e804ffc889c838c82bdbb6afa6497c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 May 2021 22:26:04 +0200 Subject: [PATCH 021/103] comments --- src/common/base_classes/Sensor.h | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index c4c0dc6f..2082230a 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -45,12 +45,15 @@ class Sensor{ public: /** * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with - * the hardware. + * the hardware. Base implementation uses the values returned by updateSensor() so that + * the same values are returned until updateSensor() is called again. */ virtual float getShaftAngle(); /** - * Get current position (in rad) including full rotations and shaft angle + * Get current position (in rad) including full rotations and shaft angle. + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. * Note that this value has limited precision as the number of rotations increases, * because the limited precision of float can't capture the large angle of the full * rotations and the small angle of the shaft angle at the same time. @@ -60,18 +63,23 @@ class Sensor{ /** * On architectures supporting it, this will return a double precision position value, * which should have improved precision for large position values. + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. */ virtual double getPreciseAngle(); /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values - * previously returned by getShaftAngle(). + * returned by updateSensor() so that it only makes sense to call this if updateSensor() + * has been called in the meantime. */ virtual float getVelocity(); /** * Get the number of full rotations + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. */ virtual int32_t getFullRotations(); @@ -107,12 +115,12 @@ class Sensor{ virtual float getSensorAngle()=0; // velocity calculation variables - float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity + float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity - long vel_angle_prev_ts=0; //!< last velocity calculation timestamp + long vel_angle_prev_ts=0; // last velocity calculation timestamp int32_t full_rotations=0; // full rotation tracking - int32_t vel_full_rotations=0; + int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation }; #endif From 42ce5511a66fbe078ee7c8a99490ae7304d548e9 Mon Sep 17 00:00:00 2001 From: "Sroka, Kamil" Date: Mon, 31 May 2021 23:19:11 +0200 Subject: [PATCH 022/103] Use float instead of double constants for improved speed --- .../bluepill_position_control.ino | 26 ++-- .../bluepill_position_control.ino | 26 ++-- .../full_control_serial.ino | 20 +-- .../full_control_serial.ino | 20 +-- .../esp32_position_control.ino | 24 ++-- .../esp32_position_control.ino | 20 +-- .../position_control/position_control.ino | 30 ++--- .../nano33IoT_velocity_control.ino | 8 +- .../single_full_control_example.ino | 18 +-- .../double_full_control_example.ino | 26 ++-- .../single_full_control_example.ino | 16 +-- .../double_full_control_example.ino | 30 ++--- .../single_full_control_example.ino | 18 +-- .../encoder/angle_control/angle_control.ino | 40 +++--- .../angle_control/angle_control.ino | 36 +++--- .../angle_control/angle_control.ino | 26 ++-- .../current_control/current_control.ino | 26 ++-- .../velocity_control/velocity_control.ino | 36 +++--- .../hall_sensor/velocity_control.ino | 36 +++--- .../velocity_control/velocity_control.ino | 26 ++-- .../full_control_serial.ino | 16 +-- .../full_control_serial.ino | 18 ++- .../full_control_serial.ino | 17 ++- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 10 +- .../osc_esp32_fullcontrol.ino | 18 +-- .../alignment_and_cogging_test.ino | 12 +- .../find_pole_pairs_number.ino | 46 +++---- .../find_pole_pairs_number.ino | 44 +++---- .../step_dir_listener_software_interrupt.ino | 16 +-- .../step_dir_motor_example.ino | 12 +- .../bldc_driver_6pwm_standalone.ino | 6 +- src/BLDCMotor.cpp | 16 +-- src/StepperMotor.cpp | 66 +++++----- src/common/base_classes/BLDCDriver.h | 22 ++-- src/common/base_classes/Sensor.cpp | 8 +- src/common/defaults.h | 40 +++--- src/common/foc_utils.cpp | 30 ++--- src/common/foc_utils.h | 46 +++---- src/common/pid.cpp | 13 +- src/communication/Commander.h | 120 +++++++++--------- src/current_sense/InlineCurrentSense.cpp | 45 ++++--- src/current_sense/LowsideCurrentSense.cpp | 32 ++--- src/current_sense/hardware_api.h | 12 +- .../hardware_specific/esp32_mcu.cpp | 6 +- .../hardware_specific/generic_mcu.cpp | 26 ++-- .../hardware_specific/samd21_mcu.cpp | 38 +++--- src/drivers/BLDCDriver3PWM.cpp | 14 +- src/drivers/BLDCDriver6PWM.cpp | 22 ++-- src/drivers/StepperDriver2PWM.cpp | 12 +- src/drivers/StepperDriver4PWM.cpp | 10 +- .../hardware_specific/atmega2560_mcu.cpp | 40 +++--- .../hardware_specific/atmega328_mcu.cpp | 42 +++--- src/drivers/hardware_specific/esp32_mcu.cpp | 88 ++++++------- src/drivers/hardware_specific/generic_mcu.cpp | 25 ++-- src/drivers/hardware_specific/rp2040_mcu.cpp | 2 +- src/drivers/hardware_specific/samd_mcu.cpp | 8 +- src/drivers/hardware_specific/stm32_mcu.cpp | 30 ++--- src/drivers/hardware_specific/teensy_mcu.cpp | 26 ++-- src/sensors/Encoder.cpp | 10 +- src/sensors/HallSensor.cpp | 19 ++- src/sensors/MagneticSensorAnalog.cpp | 18 +-- src/sensors/MagneticSensorI2C.cpp | 56 ++++---- src/sensors/MagneticSensorPWM.cpp | 16 +-- src/sensors/MagneticSensorSPI.cpp | 46 +++---- 64 files changed, 859 insertions(+), 868 deletions(-) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 267c00fb..d0768fd2 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with encoder - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -32,10 +32,10 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB, doI); + encoder.enableInterrupts(doA, doB, doI); // link the motor to the sensor motor.linkSensor(&encoder); @@ -54,20 +54,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -75,11 +75,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -97,7 +97,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 0adf4c17..1b47170d 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with magnetic sensor - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -47,37 +47,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -99,7 +99,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -112,7 +112,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 746dde3d..842f383a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -20,7 +20,7 @@ #define INH_C 11 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -45,7 +45,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -76,14 +76,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -111,7 +111,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 3e936460..6c75855a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -23,7 +23,7 @@ #define INL_C 10 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -48,7 +48,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -77,14 +77,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -112,7 +112,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 9864ca0b..3943017b 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -1,4 +1,4 @@ -/** +/** * ESP32 position motion control example with encoder * */ @@ -23,14 +23,14 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -46,20 +46,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -67,11 +67,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -92,7 +92,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -104,7 +104,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 4bd53b55..77cfeeed 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -44,37 +44,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -96,7 +96,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index e9355f65..5b5c8e40 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -1,15 +1,15 @@ /** - * + * * HMBGC position motion control example with encoder - * + * * - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6) * - Encoder is connected to A0 and A1 - * - * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library + * + * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library * - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager - * + * * See docs.simplefoc.com for more info. - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { PciManager.registerListener(&listenerB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -65,20 +65,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -86,11 +86,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -109,7 +109,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -121,7 +121,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index fc46a338..2c9baaef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -17,7 +17,7 @@ BLDCMotor motor = BLDCMotor(7); BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // instantiate the commander Commander command = Commander(SerialUSB); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } @@ -35,11 +35,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 9; //motor.P_angle.P = 20; motor.init(); 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 16c7755b..0b3075d3 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 @@ -22,7 +22,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -38,15 +38,15 @@ void setup() { motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -60,7 +60,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -68,7 +68,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 0; @@ -76,9 +76,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index e9f4e840..c048956b 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7); @@ -30,14 +30,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -55,14 +55,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -78,16 +78,16 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - + // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -99,7 +99,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 5d02ecf2..10e26c9f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -19,7 +19,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -33,15 +33,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -55,11 +55,11 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -69,7 +69,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } 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 757ae0f1..3399d2fa 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 @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7); @@ -39,14 +39,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -64,14 +64,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -87,8 +87,8 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - - + + // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_sense1); @@ -99,12 +99,12 @@ void setup() { // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -116,7 +116,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } @@ -132,4 +132,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} 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 c0f6a10b..67d1d869 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 @@ -23,7 +23,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -37,15 +37,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -59,7 +59,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -67,7 +67,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -75,9 +75,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index a8e8b997..942f73dc 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -1,27 +1,27 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -53,10 +53,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -68,7 +68,7 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] @@ -77,11 +77,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -89,9 +89,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -99,11 +99,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -121,7 +121,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -133,7 +133,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 90219b5b..4bd7163f 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -1,20 +1,20 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and hall sensor + * 1) Configure the motor and hall sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > This is for Arduino UNO example code for running angle motion control specifically * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the sensr.enableInterrupts(doA,doB,doC) and avoid using PciManger - * - * + * + * */ #include // software interrupt library @@ -46,10 +46,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -71,11 +71,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -83,9 +83,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -93,11 +93,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -115,7 +115,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -127,7 +127,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index d22289bd..752030c9 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -1,11 +1,11 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * + * */ #include @@ -42,38 +42,38 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -93,7 +93,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -106,7 +106,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index ad75b573..8b1ec96b 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 @@ -1,7 +1,7 @@ /** - * + * * Torque control example using current control loop. - * + * */ #include @@ -25,11 +25,11 @@ float target_current = 0; Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_current, cmd); } -void setup() { - +void setup() { + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -46,10 +46,10 @@ void setup() { motor.linkCurrentSense(¤t_sense); // set torque mode: - // TorqueControlType::dc_current + // TorqueControlType::dc_current // TorqueControlType::voltage // TorqueControlType::foc_current - motor.torque_controller = TorqueControlType::foc_current; + motor.torque_controller = TorqueControlType::foc_current; // set motion control loop to be used motor.controller = MotionControlType::torque; @@ -58,17 +58,17 @@ void setup() { motor.PID_current_q.I= 300; motor.PID_current_d.P= 5; motor.PID_current_d.I = 300; - motor.LPF_current_q.Tf = 0.01; - motor.LPF_current_d.Tf = 0.01; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; // foc currnet control parameters (stm/esp/due/teensy) // motor.PID_current_q.P = 5; // motor.PID_current_q.I= 1000; // motor.PID_current_d.P= 5; // motor.PID_current_d.I = 1000; - // motor.LPF_current_q.Tf = 0.002; // 1ms default - // motor.LPF_current_d.Tf = 0.002; // 1ms default + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -91,7 +91,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index a3974b38..f0f1e3e8 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -1,28 +1,28 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -59,7 +59,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -80,11 +80,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -92,11 +92,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -119,7 +119,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -131,7 +131,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 07b70dcd..1d39173a 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -1,20 +1,20 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and sensor + * 1) Configure the motor and sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : - * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { // initialize sensor sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -64,15 +64,15 @@ void setup() { // aligning voltage [V] motor.voltage_sensor_align = 3; - + // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -80,11 +80,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -107,7 +107,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -119,7 +119,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index a16385df..a57fe634 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -1,12 +1,12 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * + * + * * By using the serial terminal set the velocity value you want to motor to obtain * */ @@ -32,7 +32,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { - + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -48,11 +48,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -60,13 +60,13 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering - // default 5ms - try different values to see what is the best. + // default 5ms - try different values to see what is the best. // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -88,7 +88,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -100,7 +100,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 30d18297..186d257e 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using encoder - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -36,7 +36,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -53,15 +53,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -87,7 +87,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 7a202f94..df7b76ac 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using Hall sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -43,7 +43,7 @@ void setup() { // initialize encoder sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -61,15 +61,15 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -95,7 +95,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -112,5 +112,3 @@ void loop() { // user communication command.run(); } - - diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 97216a9e..098f6888 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using magnetic sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -51,15 +51,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the control type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -85,7 +85,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -98,8 +98,7 @@ void loop() { // velocity, position or voltage // if tatget not set in parameter uses motor.target variable motor.move(); - + // user communication command.run(); } - diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index b0a24d25..afb95294 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -93,11 +93,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 8; //motor.P_angle.P = 20; motor.init(); @@ -107,9 +107,9 @@ void setup() { } // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // angle set point variable -float target_angle = 1.0; +float target_angle = 1.0f; void motorControl(OSCMessage &msg){ diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 917f569c..7c6aa4f0 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -30,7 +30,7 @@ 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) -#define POWER_SUPPLY 7.4 +#define POWER_SUPPLY 7.4f @@ -46,8 +46,8 @@ String m2Prefix("/M2"); // we store seperate set-points for each motor, of course -float set_point1 = 0.0; -float set_point2 = 0.0; +float set_point1 = 0.0f; +float set_point2 = 0.0f; OSCErrorCode error; @@ -135,11 +135,11 @@ void setup() { motor1.linkDriver(&driver1); motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; motor1.controller = MotionControlType::velocity; - motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.P = 0.2f; motor1.PID_velocity.I = 20; - motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.D = 0.001f; motor1.PID_velocity.output_ramp = 1000; - motor1.LPF_velocity.Tf = 0.01; + motor1.LPF_velocity.Tf = 0.01f; motor1.voltage_limit = POWER_SUPPLY; motor1.P_angle.P = 20; motor1.init(); @@ -152,11 +152,11 @@ void setup() { motor2.linkDriver(&driver2); motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; motor2.controller = MotionControlType::velocity; - motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.P = 0.2f; motor2.PID_velocity.I = 20; - motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.D = 0.001f; motor2.PID_velocity.output_ramp = 1000; - motor2.LPF_velocity.Tf = 0.01; + motor2.LPF_velocity.Tf = 0.01f; motor2.voltage_limit = POWER_SUPPLY; motor2.P_angle.P = 20; motor2.init(); 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 708e2d9e..ca254a5f 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 @@ -9,7 +9,7 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); /** - * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. + * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. * It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle) * This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear * An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal. @@ -28,9 +28,9 @@ void testAlignmentAndCogging(int direction) { float stDevSum = 0; - float mean = 0.0; - float prev_mean = 0.0; - + float mean = 0.0f; + float prev_mean = 0.0f; + for (int i = 0; i < sample_count; i++) { @@ -39,7 +39,7 @@ void testAlignmentAndCogging(int direction) { motor.move(electricAngle * PI / 180); _delay(5); - // measure + // measure float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; @@ -87,7 +87,7 @@ void setup() { motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=motor.voltage_sensor_align; 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 e6a1d20b..e6187921 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 @@ -1,17 +1,17 @@ /** * 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. - * + * 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. + * + * 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 @@ -31,7 +31,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void setup() { - + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -57,20 +57,20 @@ void setup() { 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 encoder angle + // read the encoder angle float angle_begin = encoder.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.01; + motor_angle += 0.01f; motor.move(motor_angle); } _delay(1000); @@ -93,7 +93,7 @@ void setup() { Serial.print(" = "); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { 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 @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // 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(); } @@ -146,10 +146,10 @@ void loop() { // 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(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { 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 + + // reset the command buffer received_chars = ""; } } 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 cc4bd4d0..c2785c55 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 @@ -1,17 +1,17 @@ /** * 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. - * + * 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. + * + * 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 @@ -56,20 +56,20 @@ void setup() { 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 + // read the sensor angle 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.01; + motor_angle += 0.01f; sensor.getAngle(); // keep track of the overflow motor.move(motor_angle); } @@ -93,7 +93,7 @@ void setup() { Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { 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 @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // 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(); } @@ -146,10 +146,10 @@ void loop() { // 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(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { 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 + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino index d723313c..b6f0ea5f 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -1,6 +1,6 @@ /** - * A simple example of reading step/dir communication - * - this example uses software interrupts - this code is intended primarily + * A simple example of reading step/dir communication + * - this example uses software interrupts - this code is intended primarily * for Arduino UNO/Mega and similar boards with very limited number of interrupt pins */ @@ -10,11 +10,11 @@ #include -// angle +// angle float received_angle = 0; // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(4, 5, 2.0*_PI/200.0); // receive the angle in radians +StepDirListener step_dir = StepDirListener(4, 5, 2.0f*_PI/200.0); // receive the angle in radians void onStep() { step_dir.handle(); } // If no available hadware interrupt pins use the software interrupt @@ -23,21 +23,21 @@ PciListenerImp listenStep(step_dir.pin_step, onStep); void setup() { Serial.begin(115200); - + // init step and dir pins step_dir.init(); // enable software interrupts PciManager.registerListener(&listenStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling step_dir.getValue(); step_dir.attach(&received_angle); - + Serial.println(F("Step/Dir listenning.")); _delay(1000); } void loop() { - Serial.print(received_angle); + Serial.print(received_angle); Serial.print("\t"); Serial.println(step_dir.getValue()); _delay(500); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index 932edc81..f8978577 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -19,7 +19,7 @@ void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200.0); +StepDirListener step_dir = StepDirListener(A4, A5, 2.0f*_PI/200.0); void onStep() { step_dir.handle(); } void setup() { @@ -48,7 +48,7 @@ void setup() { // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -58,7 +58,7 @@ void setup() { motor.PID_velocity.output_ramp = 1000; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 10; @@ -77,12 +77,12 @@ void setup() { // init step and dir pins step_dir.init(); - // enable interrupts + // enable interrupts step_dir.enableInterrupt(onStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling motor.move(step_dir.getValue()); step_dir.attach(&motor.target); - + Serial.println(F("Motor ready.")); Serial.println(F("Listening to step/dir commands!")); _delay(1000); diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 972a74db..478d3974 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -5,7 +5,7 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { - + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -14,8 +14,8 @@ void setup() { driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply driver.voltage_limit = 12; - // daad_zone [0,1] - default 0.02 - 2% - driver.dead_zone = 0.05; + // daad_zone [0,1] - default 0.02f - 2% + driver.dead_zone = 0.05f; // driver init driver.init(); diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..d9e61aac 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -159,7 +159,7 @@ int BLDCMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -167,7 +167,7 @@ int BLDCMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -188,7 +188,7 @@ int BLDCMotor::alignSensor() { // 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.5 ) { // 0.5 is arbitrary number it can be lower or higher! + 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!")); @@ -226,7 +226,7 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -568,9 +568,9 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -597,9 +597,9 @@ float BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 183c2c65..ca0716b7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -11,7 +11,7 @@ StepperMotor::StepperMotor(int pp, float _R) // save phase resistance number phase_resistance = _R; - // torque control type is voltage by default + // torque control type is voltage by default // current and foc_current not supported yet torque_controller = TorqueControlType::voltage; } @@ -23,7 +23,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Init")); @@ -37,7 +37,7 @@ void StepperMotor::init() { if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - + // update the controller limits if(_isset(phase_resistance)){ // velocity control loop controls current @@ -52,7 +52,7 @@ void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); - + } @@ -101,14 +101,14 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); - + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } - + return exit_flag; } @@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int StepperMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - + // if unknown natural direction if(!_isset(sensor_direction)){ // check if sensor needs zero search @@ -127,7 +127,7 @@ int StepperMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -135,14 +135,14 @@ int StepperMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); - // determine the direction the sensor moved + // determine the direction the sensor moved if (mid_angle == end_angle) { if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); return 0; // failed calibration @@ -153,10 +153,10 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); sensor_direction = Direction::CW; } - // check pole pair number + // 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.5 ) { // 0.5 is arbitrary number it can be lower or higher! + 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!")); @@ -166,7 +166,7 @@ int StepperMotor::alignSensor() { // zero electric angle not known if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees + // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); @@ -185,7 +185,7 @@ int StepperMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int StepperMotor::absoluteZeroSearch() { - + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -194,7 +194,7 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -218,15 +218,15 @@ int StepperMotor::absoluteZeroSearch() { void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle + // shaft angle shaft_angle = shaftAngle(); - + // if disabled do nothing - if(!enabled) return; + if(!enabled) return; electrical_angle = electricalAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -239,7 +239,7 @@ void StepperMotor::move(float new_target) { // get angular velocity shaft_velocity = shaftVelocity(); // if disabled do nothing - if(!enabled) return; + if(!enabled) return; // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -249,7 +249,7 @@ void StepperMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance; + else voltage.q = target*phase_resistance; voltage.d = 0; break; case MotionControlType::angle: @@ -259,7 +259,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage + // if torque controlled through voltage // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -270,7 +270,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control + // if torque controlled through voltage control // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -295,12 +295,12 @@ void StepperMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Sine PWM algorithms // - space vector not implemented yet -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park transformation // angle normalization in between 0 and 2pi @@ -323,18 +323,18 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -352,9 +352,9 @@ float StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) @@ -368,12 +368,12 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; - + return Uq; } \ No newline at end of file diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 5764aa30..a657d17c 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,7 +5,7 @@ class BLDCDriver{ public: - + /** Initialise hardware */ virtual int init() = 0; /** Enable hardware */ @@ -14,26 +14,26 @@ class BLDCDriver{ virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage + float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor - + float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - - /** - * Set phase voltages to the harware - * + + /** + * Set phase voltages to the harware + * * @param Ua - phase A voltage * @param Ub - phase B voltage * @param Uc - phase C voltage */ virtual void setPwm(float Ua, float Ub, float Uc) = 0; - /** - * Set phase state, enable/disable - * + /** + * Set phase state, enable/disable + * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) @@ -41,4 +41,4 @@ class BLDCDriver{ virtual void setPhaseState(int sa, int sb, int sc) = 0; }; -#endif \ No newline at end of file +#endif diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..81e80aa2 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,7 +2,7 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) @@ -16,15 +16,15 @@ float Sensor::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..41e7c9ba 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -1,12 +1,12 @@ // default configuration values // change this file to optimal values for your application -#define DEF_POWER_SUPPLY 12.0 //!< default power supply voltage +#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage // velocity PI controller params -#define DEF_PID_VEL_P 0.5 //!< default PID controller P value -#define DEF_PID_VEL_I 10.0 //!< default PID controller I value -#define DEF_PID_VEL_D 0.0 //!< default PID controller D value -#define DEF_PID_VEL_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_VEL_P 0.5f //!< default PID controller P value +#define DEF_PID_VEL_I 10.0f //!< default PID controller I value +#define DEF_PID_VEL_D 0.0f //!< default PID controller D value +#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit // current sensing PID values @@ -14,33 +14,33 @@ // for 16Mhz controllers like Arduino uno and mega #define DEF_PID_CURR_P 2 //!< default PID controller P value #define DEF_PID_CURR_I 100 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value -#define DEF_PID_CURR_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value +#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant -#else +#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant +#else // for stm32, due, teensy, esp32 and similar #define DEF_PID_CURR_P 3 //!< default PID controller P value -#define DEF_PID_CURR_I 300.0 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value +#define DEF_PID_CURR_I 300.0f //!< default PID controller I value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value #define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample -#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable +#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable // angle P params -#define DEF_P_ANGLE_P 20.0 //!< default P controller P value -#define DEF_VEL_LIM 20.0 //!< angle velocity limit default +#define DEF_P_ANGLE_P 20.0f //!< default P controller P value +#define DEF_VEL_LIM 20.0f //!< angle velocity limit default -// index search -#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0 //!< default index search velocity +// index search +#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity // align voltage -#define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt +#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant \ No newline at end of file diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 7653f9be..58ac36e5 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,6 +1,6 @@ #include "foc_utils.h" -// int array instead of float array +// int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 @@ -13,21 +13,21 @@ const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,10 // it has to receive an angle in between 0 and 2PI float _sin(float a){ if(a < _PI_2){ - //return sine_array[(int)(199.0*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873* a)]; // float array optimized - return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized + //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; + //return sine_array[(int)(126.6873f* a)]; // float array optimized + return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized }else if(a < _PI){ - // return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873*a)]; // float array optimized - return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized + // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; + //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized + return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; + //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { - // return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; + //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } @@ -54,7 +54,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { return (shaft_angle * pole_pairs); } -// square root approximation function using +// square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat @@ -67,7 +67,7 @@ float _sqrtApprox(float number) {//low in fat y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 30e99b4f..e58fbaa0 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,31 +12,31 @@ #define _UNUSED(v) (void) (v) // utility defines -#define _2_SQRT3 1.15470053838 -#define _SQRT3 1.73205080757 -#define _1_SQRT3 0.57735026919 -#define _SQRT3_2 0.86602540378 -#define _SQRT2 1.41421356237 -#define _120_D2R 2.09439510239 -#define _PI 3.14159265359 -#define _PI_2 1.57079632679 -#define _PI_3 1.0471975512 -#define _2PI 6.28318530718 -#define _3PI_2 4.71238898038 -#define _PI_6 0.52359877559 +#define _2_SQRT3 1.15470053838f +#define _SQRT3 1.73205080757f +#define _1_SQRT3 0.57735026919f +#define _SQRT3_2 0.86602540378f +#define _SQRT2 1.41421356237f +#define _120_D2R 2.09439510239f +#define _PI 3.14159265359f +#define _PI_2 1.57079632679f +#define _PI_3 1.0471975512f +#define _2PI 6.28318530718f +#define _3PI_2 4.71238898038f +#define _PI_6 0.52359877559f #define NOT_SET -12345.0 #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -// dq current structure +// dq current structure struct DQCurrent_s { float d; float q; }; -// phase current structure +// phase current structure struct PhaseCurrent_s { float a; @@ -54,27 +54,27 @@ struct DQVoltage_s /** * Function approximating the sine calculation by using fixed size array * - execution time ~40us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _sin(float a); /** * Function approximating cosine calculation by using fixed size array * - execution time ~50us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _cos(float a); -/** - * normalizing radian angle to [0,2PI] +/** + * normalizing radian angle to [0,2PI] * @param angle - angle to be normalized - */ + */ float _normalizeAngle(float angle); - -/** - * Electrical angle calculation + +/** + * Electrical angle calculation * * @param shaft_angle - shaft angle of the motor * @param pole_pairs - number of pole pairs @@ -84,7 +84,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs); /** * Function approximating square root function * - using fast inverse square root - * + * * @param value - number */ float _sqrtApprox(float value); diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 5290d814..c887c35b 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -6,9 +6,9 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , D(D) , output_ramp(ramp) // output derivative limit [volts/second] , limit(limit) // output supply limit [volts] - , integral_prev(0.0) - , error_prev(0.0) - , output_prev(0.0) + , integral_prev(0.0f) + , error_prev(0.0f) + , output_prev(0.0f) { timestamp_prev = _micros(); } @@ -17,13 +17,13 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator() (float error){ // calculate the time from the last call unsigned long timestamp_now = _micros(); - float Ts = (timestamp_now - timestamp_prev) * 1e-6; + float Ts = (timestamp_now - timestamp_prev) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations - // proportional part + // proportional part // u_p = P *e(k) float proportional = P * error; // Tustin transform of the integral part @@ -56,4 +56,3 @@ float PIDController::operator() (float error){ timestamp_prev = timestamp_now; return output; } - diff --git a/src/communication/Commander.h b/src/communication/Commander.h index aaa55b11..13777867 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -24,14 +24,14 @@ typedef void (* CommandCallback)(char*); //!< command callback function pointer /** * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`) - * - * - This class can be used in combination with HardwareSerial instance which it would read and write + * + * - This class can be used in combination with HardwareSerial instance which it would read and write * or it can be used to parse strings that have been received from the user outside this library * - Commander class implements command protocol for few standard components of the SimpleFOC library * - FOCMotor * - PIDController * - LowPassFilter - * - Commander also provides a very simple command > callback interface that enables user to + * - Commander also provides a very simple command > callback interface that enables user to * attach a callback function to certain command id - see function add() */ class Commander @@ -40,7 +40,7 @@ class Commander /** * Default constructor receiving a serial interface that it uses to output the values to * Also if the function run() is used it uses this serial instance to read the serial for user commands - * + * * @param serial - Serial com port instance * @param eol - the end of line sentinel character * @param echo - echo last typed character (for command line feedback) @@ -49,45 +49,45 @@ class Commander Commander(char eol = '\n', bool echo = false); /** - * Function reading the serial port and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the serial port and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places - * '?' - Scan command - displays all the labels of attached nodes - */ + * '?' - Scan command - displays all the labels of attached nodes + */ void run(); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param reader - temporary stream to read user input * @param eol - temporary end of line sentinel - */ + */ void run(Stream &reader, char eol = '\n'); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param user_input - string of user inputs - */ + */ void run(char* user_input); /** * Function adding a callback to the coomander withe the command id * @param id - char command letter - * @param onCommand - function pointer void function(char*) + * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent */ void add(char id , CommandCallback onCommand, char* label = nullptr); @@ -101,48 +101,48 @@ class Commander 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 * - 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) - * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) - * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) - * 'L' - Limits + * 'D' - D current PID controller & LPF (see function pid and lpf for commands) + * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) + * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) + * 'L' - Limits * sub-commands: - * 'C' - Current - * 'U' - Voltage - * 'V' - Velocity - * 'C' - Motion control type config + * 'C' - Current + * 'U' - Voltage + * 'V' - Velocity + * 'C' - Motion control type config * sub-commands: - * 'D' - downsample motiron loop - * '0' - torque - * '1' - velocity - * '2' - angle - * 'T' - Torque control type + * '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) + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) * sub-commands: - * '0' - enable - * '1' - disable - * 'R' - Motor resistance - * 'S' - Sensor offsets + * '0' - enable + * '1' - disable + * 'R' - Motor resistance + * 'S' - Sensor offsets * sub-commands: - * 'M' - sensor offset - * 'E' - sensor electrical zero - * 'M' - Monitoring control + * 'M' - sensor offset + * 'E' - sensor electrical zero + * 'M' - Monitoring control * sub-commands: - * 'D' - downsample monitoring - * 'C' - clear monitor - * 'S' - set monitoring variables - * 'G' - get variable value - * '' - Target get/set - * + * 'D' - downsample monitoring + * 'C' - clear monitor + * 'S' - set monitoring variables + * 'G' - get variable value + * '' - Target get/set + * * - 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.5) + * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) * */ void motor(FOCMotor* motor, char* user_cmd); @@ -170,7 +170,7 @@ class Commander * Float variable scalar command interface * - 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.01 for settin *value=0.01) + * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); @@ -184,19 +184,19 @@ class Commander // helping variable for serial communication reading char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline int rec_cnt = 0; //!< number of characters receives - + // serial printing functions /** * print the string message only if verbose mode on * @param message - message to be printed */ - void printVerbose(const char* message); + void printVerbose(const char* message); /** - * Print the string message only if verbose mode on + * Print the string message only if verbose mode on * - Function handling the case for strings defined by F macro * @param message - message to be printed */ - void printVerbose(const __FlashStringHelper *message); + void printVerbose(const __FlashStringHelper *message); /** * print the numbers to the serial with desired decimal point number * @param message - number to be printed diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 879395d1..43e78b9f 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -12,7 +12,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -72,36 +72,36 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ int InlineCurrentSense::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); + _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.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _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.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }else if( ab_ratio < 0.7f ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; - pinA = pinB; + 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.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; - pinA = pinC; + pinA = pinC; pinC= tmp_pinA; gain_a *= _sign(c.c); exit_flag = 2;// signal that pins have been switched @@ -109,35 +109,35 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 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); + _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.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _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.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; - pinB = pinA; + 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.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; - pinB = pinC; + pinB = pinC; pinC = tmp_pinB; gain_b *= _sign(c.c); exit_flag = 2; // signal that pins have been switched @@ -150,7 +150,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ if(_isset(pinC)){ // set phase B active and phases A and C down driver->setPwm(0, 0, voltage); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 50; i++) { @@ -170,4 +170,3 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 4 - success but pins reconfigured and gains inverted return exit_flag; } - diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index e0df940a..383422d6 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -12,7 +12,7 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -41,9 +41,9 @@ void LowsideCurrentSense::calibrateOffsets(){ _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0; - offset_ib = offset_ib / 1000.0; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0; + offset_ia = offset_ia / 1000.0f; + offset_ib = offset_ib / 1000.0f; + if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; } // read all three phase currents (if possible 2 or 3) @@ -86,25 +86,25 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _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.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }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.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; pinA = pinC; @@ -123,24 +123,24 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _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.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }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.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; pinB = pinC; diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7b648cde..cacd8410 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -6,14 +6,14 @@ /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltageInline(const int pinA); /** * 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 @@ -21,8 +21,8 @@ float _readADCVoltageInline(const int pinA); void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); /** - * function reading an ADC value and returning the read voltage - * + * 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 @@ -33,7 +33,7 @@ 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) */ float _readADCVoltageLowSide(const int pinA); @@ -42,4 +42,4 @@ float _readADCVoltageLowSide(const int pinA); * function syncing the Driver with the ADC for the LowSide Sensing */ void _driverSyncLowSide(); -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 3f7e83b3..6d62b2e3 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -6,8 +6,8 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4095.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; //Current readings from internal current sensor amplifiers @@ -31,7 +31,7 @@ float _readADCVoltageLowSide(const int pin){ } void _startADC3PinConversionLowSide(){ - + } // function reading an ADC value and returning the read voltage diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dccd3601..241cdea4 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -3,8 +3,8 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 5.0f + #define _ADC_RESOLUTION 1024.0f #ifndef cbi #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #endif @@ -12,20 +12,20 @@ #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) #endif #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #elif defined(ESP_H) // or esp32 // do nothing implemented in esp32_mcu.h #elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 5.0f + #define _ADC_RESOLUTION 1024.0f #endif // adc counts to voltage conversion ratio @@ -46,9 +46,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ if( _isset(pinC) ) pinMode(pinC, INPUT); #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // 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 + // 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); diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 8bb6d38c..574f7f7c 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -9,8 +9,8 @@ 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 - * + * 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 @@ -18,7 +18,7 @@ static SAMDCurrentSenseADCDMA instance; void _configureADCLowSide(const int pinA,const int pinB,const int pinC) { _pinA = pinA; - _pinB = pinB; + _pinB = pinB; _pinC = pinC; freeRunning = true; instance.init(pinA, pinB, pinC); @@ -30,13 +30,13 @@ 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) */ float _readADCVoltageLowSide(const int pinA) { instance.readResults(a, b, c); - + if(pinA == _pinA) return instance.toVolts(a); if(pinA == _pinB) @@ -72,7 +72,7 @@ static void adcStopWithDMA(void); static void adcStartWithDMA(void); /** - * @brief ADC sync wait + * @brief ADC sync wait * @retval void */ static __inline__ void ADCsync() __attribute__((always_inline, unused)); @@ -82,9 +82,9 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() { - + return &instance; } @@ -136,7 +136,7 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { } void SAMDCurrentSenseADCDMA::initPins(){ - + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -145,7 +145,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); - if( _isset(pinC) ) + if( _isset(pinC) ) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); @@ -160,13 +160,13 @@ void SAMDCurrentSenseADCDMA::initPins(){ void SAMDCurrentSenseADCDMA::initADC(){ - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297f 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; @@ -215,7 +215,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ */ ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -246,9 +246,9 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); - - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -289,7 +289,7 @@ void adcStartWithDMA(void){ ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x01; + ADC->CTRLA.bit.ENABLE = 0x01; ADCsync(); } diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 11215863..7ccd09fa 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -39,7 +39,7 @@ void BLDCDriver3PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver3PWM::init() { // PWM pins pinMode(pwmA, OUTPUT); @@ -62,7 +62,7 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin -void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); @@ -73,16 +73,16 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { - + // limit the voltage in driver Ua = _constrain(Ua, 0.0, voltage_limit); Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index d51eec0a..6039f69e 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,9 +15,9 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; - + // dead zone initial - 2% - dead_zone = 0.02; + dead_zone = 0.02f; } @@ -39,9 +39,9 @@ void BLDCDriver6PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver6PWM::init() { - + // PWM pins pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); @@ -55,22 +55,22 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; - // configure 6pwm + // 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); } // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit); - Uc = _constrain(Uc, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + 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); @@ -78,6 +78,6 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 09dd3e16..47a49401 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -27,7 +27,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, dir2a = _dir2; // phase 2 direction pin // these pins are not used dir1b = NOT_SET; - dir2b = NOT_SET; + dir2b = NOT_SET; // enable_pin pin enable_pin1 = en1; @@ -59,7 +59,7 @@ void StepperDriver2PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver2PWM::init() { // PWM pins pinMode(pwm1, OUTPUT); @@ -83,22 +83,22 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { - float duty_cycle1(0.0),duty_cycle2(0.0); +void StepperDriver2PWM::setPwm(float Ua, float Ub) { + float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); - + // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); - + // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); } \ No newline at end of file diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f752ea6e..cfcb7aef 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -37,7 +37,7 @@ void StepperDriver4PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver4PWM::init() { // PWM pins @@ -59,17 +59,17 @@ int StepperDriver4PWM::init() { // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - else + else duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - + if( Ubeta > 0 ) duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); else diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 5236fb03..3d50dc45 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -15,13 +15,13 @@ void _pinHighFrequency(const int pin){ TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 10 || pin == 9 ) TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 5 || pin == 3 || pin == 2) + else if (pin == 5 || pin == 3 || pin == 2) TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 8 || pin == 7 || pin == 6) TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 44 || pin == 45 || pin == 46) TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 - + } @@ -46,23 +46,23 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// 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){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins @@ -74,25 +74,25 @@ void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const i _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } // function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm // supports Arudino/ATmega2560 int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure complementary pwm on low side @@ -121,7 +121,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// 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) { // High PWM frequency // - always max 32kHz @@ -132,12 +132,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -148,7 +148,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// 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); diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index be8dadb1..2c64ee64 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ @@ -12,9 +12,9 @@ void _pinHighFrequency(const int pin){ } if (pin == 9 || pin == 10 ) TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) + if (pin == 3 || pin == 11) TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - + } // function setting the high pwm frequency to the supplied pins @@ -42,47 +42,47 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// 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){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 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); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -117,7 +117,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// 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); @@ -130,12 +130,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -146,7 +146,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// 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); diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 0bc2d863..c87e3b70 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -6,13 +6,13 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -// empty motor slot +// 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 160e6 +#define _MCPWM_FREQ 160e6f // preferred pwm resolution default #define _PWM_RES_DEF 2048 @@ -70,7 +70,7 @@ typedef struct { } bldc_6pwm_motor_slots_t; // define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A @@ -78,19 +78,19 @@ bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { }; // define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; + }; // define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; + }; // define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A @@ -109,12 +109,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - + if (_isset(dead_zone)){ - // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); } _delay(100); @@ -128,21 +128,21 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); + prescaler = _constrain(prescaler, 0, 128); // now calculate the real resolution timer period necessary (pwm resolution) // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler mcpwm_num->timer[0].period.prescale = prescaler; mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; _delay(1); //set period mcpwm_num->timer[0].period.period = resolution_corrected; @@ -152,13 +152,13 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].period.upmethod = 0; mcpwm_num->timer[1].period.upmethod = 0; mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - // _delay(1); + _delay(1); + // _delay(1); //restart the timers mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); + _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. @@ -174,15 +174,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 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 + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_2pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -191,8 +191,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { break; } } - - // disable all the slots with the same MCPWM + + // disable all the slots with the same MCPWM // disable 3pwm bldc motor which would go in the same slot esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -219,15 +219,15 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 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 + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max bldc_3pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -236,7 +236,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM // disable 2pwm steppr motor which would go in the same slot esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -266,10 +266,10 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic 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 + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -278,7 +278,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -297,7 +297,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); @@ -314,7 +314,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // - 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 + // 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 @@ -331,7 +331,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // - 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 + // 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 @@ -349,7 +349,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - 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 + // 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 @@ -367,12 +367,12 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - 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 || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -384,7 +384,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // if no slots available if(slot_num >= 2) return -1; - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -397,7 +397,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); @@ -409,7 +409,7 @@ 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 return 0; } @@ -417,7 +417,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // - 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 + // 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 // se the PWM on the slot timers diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 6549de07..a2c98aac 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -53,34 +53,34 @@ __attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, co } -// function setting the pwm duty cycle to the hardware +// 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){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } -// function setting the pwm duty cycle to the hardware +// 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){ // 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(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -100,4 +100,3 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(pinC_l); return; } - diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 097b7a6d..7be8faaf 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -147,7 +147,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in inline float swDti(float val, float dt) { float ret = dt+val; - if (ret>1.0) ret = 1.0; + if (ret>1.0) ret = 1.0f; return ret; } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 1b035dfd..e1d38a80 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -704,7 +704,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i 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)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + 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); } @@ -715,7 +715,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i 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.0; // no off-time is better than too-short dead-time + 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); } @@ -726,7 +726,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i 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.0; // no off-time is better than too-short dead-time + 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); } @@ -866,5 +866,3 @@ void printTCCConfiguration(tccConfiguration& info) { #endif #endif - - diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 3f213711..b0ac00e7 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -27,12 +27,12 @@ void _setPwm(int ulPin, uint32_t value, int resolution) } -// init pin pwm +// init pin pwm HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) { PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); @@ -61,7 +61,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; @@ -129,17 +129,17 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in PinName wlPinName = digitalPinToPinName(pinC_l); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); } 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); @@ -148,20 +148,20 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); // dead time is set in nanoseconds - uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); HT->pause(); HT->refresh(); - HT->resume(); + HT->resume(); return HT; } // returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns 1 all the channels belong to the same timer - hardware inverted channels // returns -1 if neither - avoid configuring - error!!! int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ PinName nameAH = digitalPinToPinName(pinA_h); @@ -177,7 +177,7 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else @@ -230,7 +230,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); } @@ -315,11 +315,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i _setPwm(pinC_h, _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_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_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_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); break; } diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 5f70db4f..67bb0ad1 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +8,7 @@ // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); - analogWriteFrequency(pin, freq); + analogWriteFrequency(pin, freq); } @@ -42,25 +42,25 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); - _setHighFrequency(pwm_frequency, pinD); + _setHighFrequency(pwm_frequency, pinD); } -// function setting the pwm duty cycle to the hardware +// 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){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 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){ // 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(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware @@ -68,10 +68,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // 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(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..4d110d7d 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -112,12 +112,12 @@ float Encoder::getVelocity(){ // timestamp long timestamp_us = _micros(); // sampling time calculation - float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6; + float Th = (timestamp_us - pulse_timestamp) * 1e-6f; long dN = pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) @@ -129,8 +129,8 @@ float Encoder::getVelocity(){ float dt = Ts + prev_Th - Th; pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; - // if more than 0.05 passed in between impulses - if ( Th > 0.1) pulse_per_second = 0; + // if more than 0.05f passed in between impulses + if ( Th > 0.1f) pulse_per_second = 0; // velocity calculation float velocity = pulse_per_second / ((float)cpr) * (_2PI); diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..a7a9f005 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -7,14 +7,14 @@ - pp - pole pairs */ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ - + // hardware pins pinA = _hallA; pinB = _hallB; pinC = _hallC; // hall has 6 segments per electrical revolution - cpr = _pp * 6; + cpr = _pp * 6; // extern pullup as default pullup = Pullup::USE_EXTERN; @@ -40,12 +40,12 @@ void HallSensor::handleC() { /** * Updates the state and sector following an interrupt - */ + */ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); - + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed if (new_hall_state == hall_state) { return; @@ -74,7 +74,7 @@ void HallSensor::updateState() { } else { pulse_diff = 0; } - + pulse_timestamp = new_pulse_timestamp; total_interrupts++; old_direction = direction; @@ -87,7 +87,7 @@ void HallSensor::updateState() { * ... // for debug or call driver directly? * } * sensor.attachSectorCallback(onSectorChange); - */ + */ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } @@ -107,12 +107,12 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0f); } } -// HallSensor initialisation of the hardware pins +// HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ // initialise the electrical rotations to 0 @@ -134,7 +134,7 @@ void HallSensor::init(){ B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); - + pulse_timestamp = _micros(); } @@ -149,4 +149,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..f67d432b 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -6,7 +6,7 @@ * @param _max_raw_count the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096) */ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){ - + pinAnalog = _pinAnalog; cpr = _max_raw_count - _min_raw_count; @@ -26,30 +26,30 @@ void MagneticSensorAnalog::init(){ // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorAnalog::getAngle(){ // raw data from the sensor - raw_count = getRawCount(); + raw_count = getRawCount(); int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - // calculate velocity here + // calculate velocity here long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; + float Ts = ( now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index c9029f6e..56d202dc 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -19,21 +19,21 @@ MagneticSensorI2CConfig_s AS5048_I2C = { // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address -// @param _bit_resolution bit resolution of the sensor +// @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ // chip I2C address - chip_address = _chip_address; + chip_address = _chip_address; // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) cpr = pow(2, _bit_resolution); - + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -43,13 +43,13 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, } MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; + chip_address = config.chip_address; // angle read register of the magnetic sensor angle_register_msb = config.angle_register; // register maximum value (counts per revolution) cpr = pow(2, config.bit_resolution); - + int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; // extraction masks @@ -61,36 +61,36 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; - + // I2C communication begin wire->begin(); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; } @@ -98,15 +98,15 @@ float MagneticSensorI2C::getAngle(){ float MagneticSensorI2C::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; @@ -119,7 +119,7 @@ int MagneticSensorI2C::getRawCount(){ return (int)MagneticSensorI2C::read(angle_register_msb); } -// I2C functions +// I2C functions /* * Read a register from the sensor * Takes the address of the register as a uint8_t @@ -133,14 +133,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { wire->beginTransmission(chip_address); wire->write(angle_reg_msb); wire->endTransmission(false); - + // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - // depending on the sensor architecture there are different combinations of + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB @@ -151,7 +151,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { /* * Checks whether other devices have locked the bus. Can clear SDA locks. -* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda * e.g some stm32 boards with AS5600 chips * Takes the sda_pin and scl_pin * Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW @@ -160,7 +160,7 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { pinMode(scl_pin, INPUT_PULLUP); pinMode(sda_pin, INPUT_PULLUP); - delay(250); + delay(250); if (digitalRead(scl_pin) == LOW) { // Someone else has claimed master!"); @@ -179,15 +179,15 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { } pinMode(sda_pin, INPUT); delayMicroseconds(20); - if (digitalRead(sda_pin) == LOW) { + if (digitalRead(sda_pin) == LOW) { // SDA still blocked - return 2; - } + return 2; + } _delay(1000); } // SDA is clear (HIGH) pinMode(sda_pin, INPUT); pinMode(scl_pin, INPUT); - - return 0; + + return 0; } diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index da4ebf89..a067eb09 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -13,7 +13,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m cpr = _max_raw_count - _min_raw_count; min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - + // define if the sensor uses interrupts is_interrupt_based = false; @@ -23,7 +23,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m void MagneticSensorPWM::init(){ - + // initial hardware pinMode(pinPWM, INPUT); @@ -36,23 +36,23 @@ void MagneticSensorPWM::init(){ raw_count_prev = getRawCount(); } -// get current angle (rad) +// get current angle (rad) float MagneticSensorPWM::getAngle(){ // raw data from sensor raw_count = getRawCount(); - + int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; // calculate velocity here long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; + float Ts = (now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass @@ -80,7 +80,7 @@ int MagneticSensorPWM::getRawCount(){ void MagneticSensorPWM::handlePWM() { // unsigned long now_us = ticks(); unsigned long now_us = _micros(); - + // if falling edge, calculate the pulse length if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 5ee9ecf9..ce9a13cb 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -8,7 +8,7 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x3FFF, - .data_start_bit = 13, + .data_start_bit = 13, .command_rw_bit = 14, .command_parity_bit = 15 }; @@ -22,19 +22,19 @@ MagneticSensorSPIConfig_s MA730_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x0000, - .data_start_bit = 15, + .data_start_bit = 15, .command_rw_bit = 0, // not required .command_parity_bit = 0 // parity not implemented }; // MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register) -// cs - SPI chip select pin +// cs - SPI chip select pin // _bit_resolution sensor resolution bit number // _angle_register - (optional) angle read register - default 0x3FFF MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ - - chip_select_pin = cs; + + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -42,14 +42,14 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; - + command_parity_bit = 15; // for backwards compatibilty command_rw_bit = 14; // for backwards compatibilty data_start_bit = 13; // for backwards compatibilty } MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ - chip_select_pin = cs; + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -57,7 +57,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; - + command_parity_bit = config.command_parity_bit; // for backwards compatibilty command_rw_bit = config.command_rw_bit; // for backwards compatibilty data_start_bit = config.data_start_bit; // for backwards compatibilty @@ -72,7 +72,7 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //setup pins pinMode(chip_select_pin, OUTPUT); - + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); // do any architectures need to set the clock divider for SPI? Why was this in the code? @@ -81,31 +81,31 @@ void MagneticSensorSPI::init(SPIClass* _spi){ digitalWrite(chip_select_pin, HIGH); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorSPI::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; } @@ -113,9 +113,9 @@ float MagneticSensorSPI::getAngle(){ float MagneticSensorSPI::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); @@ -134,7 +134,7 @@ int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); } -// SPI functions +// SPI functions /** * Utility function used to calculate even parity of word */ @@ -174,13 +174,13 @@ word MagneticSensorSPI::read(word angle_register){ digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - + #if defined( ESP_H ) // if ESP32 board delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif - + //Now read the response digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); @@ -188,7 +188,7 @@ word MagneticSensorSPI::read(word angle_register){ //SPI - end transaction spi->endTransaction(); - + register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word const static word data_mask = 0xFFFF >> (16 - bit_resolution); From 709a558062bf3c8670561150f6e8e7799df03787 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 3 Jun 2021 23:51:34 +0200 Subject: [PATCH 023/103] add a (optional) init method to sensor base class --- src/common/base_classes/Sensor.cpp | 15 ++++++++++++++- src/common/base_classes/Sensor.h | 7 +++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 21f6797f..3bb6c734 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -30,6 +30,19 @@ float Sensor::getVelocity() { return vel; } +void Sensor::init() { + // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero) + getSensorAngle(); // call once + delayMicroseconds(1); + vel_angle_prev = getSensorAngle(); // call again + vel_angle_prev_ts = _micros(); + delay(1); + getSensorAngle(); // call once + delayMicroseconds(1); + angle_prev = getSensorAngle(); // call again + angle_prev_ts = _micros(); +} + float Sensor::getShaftAngle() { return angle_prev; @@ -57,4 +70,4 @@ int32_t Sensor::getFullRotations() { int Sensor::needsSearch() { return 0; // default false -} \ No newline at end of file +} diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 2082230a..78193158 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -113,6 +113,13 @@ class Sensor{ * Use updateSensor() when calling from outside code. */ virtual float getSensorAngle()=0; + /** + * Call Sensor::init() from your sensor subclass's init method if you want smoother startup + * The base class init() method calls getSensorAngle() several times to initialize the internal fields + * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls + * to sensor.getAngle() and sensor.getVelocity() + */ + virtual void init(); // velocity calculation variables float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity From ce75219c1cd849d4ffae0dc32db8ce1fe266cc80 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 08:14:25 +0200 Subject: [PATCH 024/103] FEAT esp32 adc wrapper --- keywords.txt | 1 + src/SimpleFOC.h | 1 + src/current_sense/LowsideCurrentSense.cpp | 18 +- .../hardware_specific/atmega_mcu.cpp | 55 ++++ .../hardware_specific/due_mcu.cpp | 38 +++ .../hardware_specific/esp32_adc_driver.cpp | 240 ++++++++++++++++++ .../hardware_specific/esp32_adc_driver.h | 104 ++++++++ .../hardware_specific/esp32_mcu.cpp | 41 ++- .../hardware_specific/generic_mcu.cpp | 64 ++--- .../hardware_specific/stm32_mcu.cpp | 39 +++ .../hardware_specific/teensy_mcu.cpp | 37 +++ 11 files changed, 581 insertions(+), 57 deletions(-) create mode 100644 src/current_sense/hardware_specific/atmega_mcu.cpp create mode 100644 src/current_sense/hardware_specific/due_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.h create mode 100644 src/current_sense/hardware_specific/stm32_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy_mcu.cpp diff --git a/keywords.txt b/keywords.txt index 77152a36..8201c144 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ StepperDriver KEYWORD1 PIDController KEYWORD1 LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 +LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index abebf03d..cad7fdbb 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -27,22 +27,24 @@ void LowsideCurrentSense::init(){ calibrateOffsets(); } // Function finding zero offsets of the ADC -void LowsideCurrentSense::calibrateOffsets(){ +void LowsideCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + // find adc offset = zero current voltage - offset_ia =0; - offset_ib= 0; - offset_ic= 0; + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) - for (int i = 0; i < 1000; i++) { + for (int i = 0; i < calibration_rounds; i++) { offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0; - offset_ib = offset_ib / 1000.0; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0; + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } // read all three phase currents (if possible 2 or 3) diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp new file mode 100644 index 00000000..021da52f --- /dev/null +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -0,0 +1,55 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) + +#define _ADC_VOLTAGE 5.0 +#define _ADC_RESOLUTION 1024.0 + +// 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 +#ifndef sbi + #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){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // 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); +} + +#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 new file mode 100644 index 00000000..c6207c05 --- /dev/null +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -0,0 +1,38 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// 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){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// 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); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp new file mode 100644 index 00000000..7a6ded1c --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -0,0 +1,240 @@ +// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at + +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if defined(ESP_H) + +#include "esp32_adc_driver.h" +#include "Arduino.h" + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "rom/ets_sys.h" +#include "esp_attr.h" +#include "esp_intr.h" +#include "soc/rtc_io_reg.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" + +static uint8_t __analogAttenuation = 3;//11db +static uint8_t __analogWidth = 3;//12 bits +static uint8_t __analogCycles = 8; +static uint8_t __analogSamples = 0;//1 sample +static uint8_t __analogClockDiv = 1; + +// Width of returned answer () +static uint8_t __analogReturnedWidth = 12; + +void __analogSetWidth(uint8_t bits){ + if(bits < 9){ + bits = 9; + } else if(bits > 12){ + bits = 12; + } + __analogReturnedWidth = bits; + __analogWidth = bits - 9; + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); + + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); +} + +void __analogSetCycles(uint8_t cycles){ + __analogCycles = cycles; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); +} + +void __analogSetSamples(uint8_t samples){ + if(!samples){ + return; + } + __analogSamples = samples - 1; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); +} + +void __analogSetClockDiv(uint8_t clockDiv){ + if(!clockDiv){ + return; + } + __analogClockDiv = clockDiv; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); +} + +void __analogSetAttenuation(uint8_t attenuation) +{ + __analogAttenuation = attenuation & 3; + uint32_t att_data = 0; + int i = 10; + while(i--){ + att_data |= __analogAttenuation << (i * 2); + } + WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels + WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); +} + +void IRAM_ATTR __analogInit(){ + static bool initialized = false; + if(initialized){ + return; + } + + __analogSetAttenuation(__analogAttenuation); + __analogSetCycles(__analogCycles); + __analogSetSamples(__analogSamples + 1);//in samples + __analogSetClockDiv(__analogClockDiv); + __analogSetWidth(__analogWidth + 9);//in bits + + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); + + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== + + initialized = true; +} + +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0 || attenuation > 3){ + return ; + } + __analogInit(); + if(channel > 7){ + SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); + } else { + SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + } +} + +bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + int8_t pad = digitalPinToTouchChannel(pin); + if(pad >= 0){ + uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); + if(touch & (1 << pad)){ + touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) + | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) + | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); + WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); + } + } else if(pin == 25){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 + } else if(pin == 26){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 + } + + pinMode(pin, ANALOG); + + __analogInit(); + return true; +} + +bool IRAM_ATTR __adcStart(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_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); + } + return true; +} + +bool IRAM_ATTR __adcBusy(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 7){ + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + } + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); +} + +uint16_t IRAM_ATTR __adcEnd(uint8_t pin) +{ + + uint16_t value = 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return 0;//not adc pin + } + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + 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); +} + +void __analogReadResolution(uint8_t bits) +{ + if(!bits || bits > 16){ + return; + } + __analogSetWidth(bits); // hadware from 9 to 12 + __analogReturnedWidth = bits; // software from 1 to 16 +} + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ + if(!__adcAttachPin(pin) || !__adcStart(pin)){ + return 0; + } + return __adcEnd(pin); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h new file mode 100644 index 00000000..aff9348b --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -0,0 +1,104 @@ +/* + Arduino.h - Main include file for the Arduino SDK + Copyright (c) 2005-2013 Arduino Team. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#if !defined(MAIN_ESP32_HAL_ADC_DRIVER_H_) && defined(ESP_H) +#define MAIN_ESP32_HAL_ADC_DRIVER_H_ + +#include "esp32-hal.h" + + +/* + * Get ADC value for pin + * */ +uint16_t adcRead(uint8_t pin); + +/* + * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). + * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. + * Range is 1 - 16 + * + * Note: compatibility with Arduino SAM + */ +void __analogReadResolution(uint8_t bits); + +/* + * Sets the sample bits and read resolution + * Default is 12bit (0 - 4095) + * Range is 9 - 12 + * */ +void __analogSetWidth(uint8_t bits); + +/* + * Set number of cycles per sample + * Default is 8 and seems to do well + * Range is 1 - 255 + * */ +void __analogSetCycles(uint8_t cycles); + +/* + * Set number of samples in the range. + * Default is 1 + * Range is 1 - 255 + * This setting splits the range into + * "samples" pieces, which could look + * like the sensitivity has been multiplied + * that many times + * */ +void __analogSetSamples(uint8_t samples); + +/* + * Set the divider for the ADC clock. + * Default is 1 + * Range is 1 - 255 + * */ +void __analogSetClockDiv(uint8_t clockDiv); + +/* + * Set the attenuation for all channels + * Default is 11db + * */ +void __analogSetAttenuation(uint8_t attenuation); + +/* + * Set the attenuation for particular pin + * Default is 11db + * */ +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); + +/* + * Attach pin to ADC (will also clear any other analog mode that could be on) + * */ +bool __adcAttachPin(uint8_t pin); + +/* + * Start ADC conversion on attached pin's bus + * */ +bool __adcStart(uint8_t pin); + +/* + * Check if conversion on the pin's ADC bus is currently running + * */ +bool __adcBusy(uint8_t pin); + +/* + * Get the result of the conversion (will wait if it have not finished) + * */ +uint16_t __adcEnd(uint8_t pin); + +#endif /* MAIN_ESP32_HAL_ADC_H_ */ \ 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 7f84f9a9..39d18dbf 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -6,6 +6,11 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" +#include +#include + +#include "esp32_adc_driver.h" + #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 4095.0 @@ -30,7 +35,6 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } - // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; @@ -43,6 +47,10 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ } void _driverSyncLowSide(){ + // 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 + + 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 //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt @@ -51,30 +59,51 @@ void _driverSyncLowSide(){ // Read currents when interrupt is triggered static void IRAM_ATTR isr_handler(void*){ +// 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_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 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + // digitalWrite(14,HIGH); if(mcpwm_intr_status_0 > 0 && currentState == 1){ - a1 = analogRead(_pinA); - //a2 = analogRead(_pinB); + a1 = adcRead(_pinA); + // a2 = analogRead(_pinA); currentState = 2; } else if(mcpwm_intr_status_1 > 0 && currentState == 2){ - a2 = analogRead(_pinB); - //a3 = analogRead(_pinC); + a2 = adcRead(_pinB); + // a3 = analogRead(_pinB); currentState = 1; } /* else if(mcpwm_intr_status_2 > 0 && currentState == 3){ - a3 = analogRead(_pinC); + a3 = adcRead(_pinC); //a1 = analogRead(_pinA); currentState = 1; }*/ + // digitalWrite(14,LOW); + // 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; 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; //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = adcRead(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/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dccd3601..c5bbcc10 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,56 +1,34 @@ #include "../hardware_api.h" - - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 - #ifndef cbi - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) - #endif - #ifndef sbi - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - #endif -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(ESP_H) // or esp32 - // do nothing implemented in esp32_mcu.h - -#elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 -#endif - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +// if the mcu doen't have defiend analogRead +// __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + return raw_adc * 5.0/1024; } +// function reading an ADC value and returning the read voltage +__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} + +// 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); +} + - #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // 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); - #endif -} \ No newline at end of file +// sync driver and the adc +__attribute__((weak)) void _driverSyncLowSide(){ } diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..82d66647 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,39 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// 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){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// 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); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..dc6048e2 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// 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){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// 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); +} + +#endif \ No newline at end of file From a8acae3aad726f4c821bd1957d68f6d1b66eef07 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 08:53:55 +0200 Subject: [PATCH 025/103] merge of esp32 low side wrapper with current dev --- src/SimpleFOC.h | 2 +- src/current_sense/LowsideCurrentSense.cpp | 2 + src/current_sense/hardware_api.h | 3 + .../hardware_specific/generic_mcu.cpp | 2 + .../hardware_specific/samd21_mcu.cpp | 308 ++++++++++++++++++ .../hardware_specific/samd21_mcu.h | 67 ++++ src/drivers/hardware_specific/samd21_mcu.cpp | 34 +- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +- src/drivers/hardware_specific/samd_mcu.cpp | 162 ++++----- src/drivers/hardware_specific/samd_mcu.h | 5 +- src/sensors/MagneticSensorSPI.cpp | 19 +- 11 files changed, 499 insertions(+), 125 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 02e215b9..4f9f1108 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,7 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" -#include "current_sense/LowSideCurrentSense.h" +#include "current_sense/LowsideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index cad7fdbb..88213b2e 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -36,6 +36,7 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ic = 0; // 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); @@ -50,6 +51,7 @@ void LowsideCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) 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 diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 4c4c1086..7b648cde 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,6 +28,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); + +void _startADC3PinConversionLowSide(); + /** * function reading an ADC value and returning the read voltage * diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index c5bbcc10..894af62c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -32,3 +32,5 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } + +__attribute__((weak)) void _startADC3PinConversionLowSide(){ } \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..8bb6d38c --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,308 @@ +#ifdef _SAMD21_ + +#include "samd21_mcu.h" +#include "../hardware_api.h" + + +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) +{ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} +void _startADC3PinConversionLowSide() +{ + instance.startADCScan(); +} +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA) +{ + instance.readResults(a, b, c); + + if(pinA == _pinA) + return instance.toVolts(a); + if(pinA == _pinB) + return instance.toVolts(b); + if(pinA == _pinC) + return instance.toVolts(c); + + return NAN; +} + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide() +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); + instance.startADCScan(); + //TODO: hook with PWM interrupts +} + + + + + + + + + + + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +{ + + return &instance; +} + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +{ +} + +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +{ + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; + this->pinAREF = pinAREF; + this->channelDMA = channelDMA; + this->voltageAREF = voltageAREF; + this->maxCountsADC = 1 << adcBits; + countsToVolts = ( voltageAREF / maxCountsADC ); + + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() +} + + +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); +} + +bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; + if(_isset(pinC)) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[ainC]; + } + return true; +} + + +float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { + return counts * countsToVolts; +} + +void SAMDCurrentSenseADCDMA::initPins(){ + + pinMode(pinAREF, INPUT); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); + if( _isset(pinC) ) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + pinMode(pinC, INPUT); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); + } + + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; + +} + +void SAMDCurrentSenseADCDMA::initADC(){ + + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //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; + ADCsync(); // ref 31.6.16 + + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); + +void SAMDCurrentSenseADCDMA::initDMA() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + + +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + + +int iii = 0; + +void adcStopWithDMA(void){ + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x00; + // ADCsync(); + // if(iii++ % 1000 == 0) + // { + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); + // } + + +} + +void adcStartWithDMA(void){ + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); + ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); +} + +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adcStopWithDMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..c0cec74a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,67 @@ +#ifdef _SAMD21_ + +#ifndef CURRENT_SENSE_SAMD21_H +#define CURRENT_SENSE_SAMD21_H + +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif + +#include + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + + +class SAMDCurrentSenseADCDMA +{ + +public: + static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); + SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); + void startADCScan(); + bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + float toVolts(uint16_t counts); +private: + + void adcToDMATransfer(void *rxdata, uint32_t hwords); + + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel + bool freeRunning; + + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); + +}; + +#endif + + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ 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 - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + 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.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 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 - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,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 - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("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 @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + 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("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b3d82dee..5ee9ecf9 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -75,11 +75,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif + // 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); // velocity calculation init @@ -170,35 +167,27 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); #if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word From ae31910086b0ea1f53684f329b206b8025f647d7 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 09:06:14 +0200 Subject: [PATCH 026/103] added weak definitions for - esp32 redefine analogRead --- src/current_sense/hardware_specific/generic_mcu.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 894af62c..86c5c716 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,7 +1,9 @@ #include "../hardware_api.h" // if the mcu doen't have defiend analogRead -// __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; +#if defined(ESP_H) + __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; +#endif // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ @@ -32,5 +34,4 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } - __attribute__((weak)) void _startADC3PinConversionLowSide(){ } \ No newline at end of file From 644389bad3a9bed6d114a9455681578173c651ff Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 14:50:26 +0200 Subject: [PATCH 027/103] FEAT fix of esp32 driver + added support for per phase lpf for current sensing --- keywords.txt | 3 ++ src/common/defaults.h | 5 ++- src/current_sense/InlineCurrentSense.cpp | 6 +-- src/current_sense/InlineCurrentSense.h | 9 ++++- src/current_sense/LowsideCurrentSense.cpp | 6 +-- src/current_sense/LowsideCurrentSense.h | 7 ++++ src/current_sense/hardware_api.h | 7 ++-- .../hardware_specific/esp32_adc_driver.cpp | 19 +--------- .../hardware_specific/esp32_adc_driver.h | 28 +++----------- .../hardware_specific/esp32_mcu.cpp | 9 +++-- .../hardware_specific/generic_mcu.cpp | 5 --- .../hardware_specific/samd_mcu.cpp | 37 +++++++++++++++++++ src/drivers/hardware_specific/generic_mcu.cpp | 5 +++ 13 files changed, 87 insertions(+), 59 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd_mcu.cpp diff --git a/keywords.txt b/keywords.txt index 8201c144..1d8156f5 100644 --- a/keywords.txt +++ b/keywords.txt @@ -49,6 +49,9 @@ LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 LPF_angle KEYWORD2 +lpf_a KEYWORD2 +lpf_b KEYWORD2 +lpf_c KEYWORD2 MotionControlType KEYWORD1 TorqueControlType KEYWORD1 diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..63357fc5 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,4 +43,7 @@ // align voltage #define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant + +// current sense default parameters +#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 879395d1..454975dc 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -50,9 +50,9 @@ 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 = lpf_a(_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps + current.b = lpf_b(_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : lpf_c(_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index da46d104..2aaca4a4 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -4,7 +4,9 @@ #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" @@ -33,8 +35,13 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - private: + // per phase low pass fileters + LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: + // hardware variables int pinA; //!< pin A analog pin for current measurement int pinB; //!< pin B analog pin for current measurement diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 88213b2e..d643a056 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -52,9 +52,9 @@ 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 = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps + current.b = lpf_b(_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : lpf_b(_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 2300d5f6..38c6d1a2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -4,8 +4,10 @@ #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/base_classes/FOCMotor.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,6 +36,11 @@ class LowsideCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain + // per phase low pass fileters + LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: // hardware variables diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7b648cde..126f4342 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,9 +28,6 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); - -void _startADC3PinConversionLowSide(); - /** * function reading an ADC value and returning the read voltage * @@ -42,4 +39,8 @@ float _readADCVoltageLowSide(const int pinA); * function syncing the Driver with the ADC for the LowSide Sensing */ void _driverSyncLowSide(); + + +void _startADC3PinConversionLowSide(); + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 7a6ded1c..01e1ca60 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -1,21 +1,6 @@ -// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at - -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#if defined(ESP_H) - #include "esp32_adc_driver.h" -#include "Arduino.h" + +#ifdef ESP_H #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 aff9348b..a01eef19 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -1,28 +1,11 @@ -/* - Arduino.h - Main include file for the Arduino SDK - Copyright (c) 2005-2013 Arduino Team. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - */ -#if !defined(MAIN_ESP32_HAL_ADC_DRIVER_H_) && defined(ESP_H) -#define MAIN_ESP32_HAL_ADC_DRIVER_H_ -#include "esp32-hal.h" +#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#include "Arduino.h" +#ifdef ESP_H /* * Get ADC value for pin * */ @@ -101,4 +84,5 @@ bool __adcBusy(uint8_t pin); * */ uint16_t __adcEnd(uint8_t pin); -#endif /* MAIN_ESP32_HAL_ADC_H_ */ \ No newline at end of file +#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ +#endif /* ESP32 */ \ 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 39d18dbf..c880c56a 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) +#ifdef ESP_H #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" @@ -11,8 +11,8 @@ #include "esp32_adc_driver.h" -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4095.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; //Current readings from internal current sensor amplifiers @@ -98,8 +98,9 @@ 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 int pinA,const int pinB, const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 86c5c716..acae178c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,10 +1,5 @@ #include "../hardware_api.h" -// if the mcu doen't have defiend analogRead -#if defined(ESP_H) - __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; -#endif - // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..0c953ea1 --- /dev/null +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// 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){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// 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); +} + +#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 6549de07..7f39f4a3 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,5 +1,10 @@ #include "../hardware_api.h" +// if the mcu doen't have defiend analogWrite +#if defined(ESP_H) + __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; +#endif + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic From dc51797290c952361240f364f38ce89ebe2de997 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:17:02 +0200 Subject: [PATCH 028/103] FEAT fix merge bugs --- src/current_sense/LowsideCurrentSense.cpp | 7 ++++--- .../hardware_specific/atmega_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/due_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/esp32_mcu.cpp | 7 ------- .../hardware_specific/generic_mcu.cpp | 2 +- .../hardware_specific/samd21_mcu.cpp | 15 ++++++++------- src/current_sense/hardware_specific/samd_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/stm32_mcu.cpp | 4 ++-- .../hardware_specific/teensy_mcu.cpp | 4 ++-- 9 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 346d32cf..086edbc9 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -43,12 +43,13 @@ void LowsideCurrentSense::calibrateOffsets(){ _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0f; - offset_ib = offset_ib / 1000.0f; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } // read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 021da52f..75265c03 100644 --- a/src/current_sense/hardware_specific/atmega_mcu.cpp +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) -#define _ADC_VOLTAGE 5.0 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 5.0f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp index c6207c05..ef450b35 100644 --- a/src/current_sense/hardware_specific/due_mcu.cpp +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__arm__) && defined(__SAM3X8E__) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 5f5adff4..c880c56a 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -35,13 +35,6 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } -<<<<<<< HEAD -void _startADC3PinConversionLowSide(){ - -} - -======= ->>>>>>> dev // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index ca8c7d4e..ecc0b79b 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -3,7 +3,7 @@ // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * 5.0/1024; + return raw_adc * 5.0f/1024.0f; } // function reading an ADC value and returning the read voltage diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f6f481f6..af2c0048 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -19,23 +19,24 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) { _pinA = pinA; _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} void _startADC3PinConversionLowSide() { instance.startADCScan(); } /** * function reading an ADC value and returning the read voltage -<<<<<<< HEAD - * -======= * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ float _readADCVoltageLowSide(const int pinA) +{ instance.readResults(a, b, c); -<<<<<<< HEAD - -======= ->>>>>>> dev if(pinA == _pinA) return instance.toVolts(a); if(pinA == _pinB) diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp index 0c953ea1..550d7bee 100644 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -2,8 +2,8 @@ #if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp index 82d66647..19833e11 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -3,8 +3,8 @@ #if defined(_STM32_DEF_) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp index dc6048e2..606ce618 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution From 2bda51f7422f3c1660ac880b1b2ce9afef3d36fa Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:27:45 +0200 Subject: [PATCH 029/103] merge conflict hardware_api --- src/current_sense/hardware_api.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 126f4342..49f2e7b3 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,6 +28,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); + +void _startADC3PinConversionLowSide(); + /** * function reading an ADC value and returning the read voltage * From 1a6269d6e1e7c7aaa29699fcffe8569c7f475e48 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:30:15 +0200 Subject: [PATCH 030/103] preparation for 2.1.2 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index af8559f1..0d89185d 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.1 +version=2.1.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 7382429330a9a3d1cbe07d4dcf3171f03fac65fc Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 16:07:38 +0200 Subject: [PATCH 031/103] FEAT due to the low memory of the Arduino UNO remove the per phase filtering for now --- src/current_sense/InlineCurrentSense.cpp | 6 +++--- src/current_sense/InlineCurrentSense.h | 8 ++++---- src/current_sense/LowsideCurrentSense.cpp | 6 +++--- src/current_sense/LowsideCurrentSense.h | 8 ++++---- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index eb6493ab..43e78b9f 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = lpf_a(_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps - current.b = lpf_b(_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : lpf_c(_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps + 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 return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 2aaca4a4..615c4733 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -35,10 +35,10 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - // per phase low pass fileters - LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter private: diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 086edbc9..18aefd49 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -52,9 +52,9 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = lpf_b(_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : lpf_b(_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + 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 return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 38c6d1a2..9618b572 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -36,10 +36,10 @@ class LowsideCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - // per phase low pass fileters - LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter private: From b860f4d3c4542b09f35247c5bcfc89da449bf181 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 09:21:44 +0200 Subject: [PATCH 032/103] pwm sensor allowed arduino uno attach interrupt --- src/sensors/MagneticSensorPWM.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index a067eb09..713bcc2a 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -95,8 +95,6 @@ void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){ // declare it's interrupt based is_interrupt_based = true; - #if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega168__) && !defined(__AVR_ATmega328PB__) && !defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 - // enable interrupts on pwm input pin - attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); - #endif + // enable interrupts on pwm input pin + attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); } \ No newline at end of file From d6b33f4c6c6ae94937eadc0a62c4cd3c1755b436 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 09:22:28 +0200 Subject: [PATCH 033/103] FIX set voltage d to 0 when in torque mode --- src/BLDCMotor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d9e61aac..93fcee17 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -313,11 +313,13 @@ void BLDCMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage) // if voltage torque control + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; else voltage.q = target*phase_resistance; - else + voltage.d = 0; + }else{ current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: // angle set point From a22c2bcd6fc0025229eae5ac5bd3a847663faae3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 10:11:22 +0200 Subject: [PATCH 034/103] API change stepperdriver2pwm --- .../smartstepper_control.ino | 61 +++++++++++++++++++ .../stepper_driver_2pwm_standalone.ino | 6 +- src/drivers/StepperDriver2PWM.cpp | 14 ++--- src/drivers/StepperDriver2PWM.h | 8 +-- 4 files changed, 75 insertions(+), 14 deletions(-) create mode 100644 examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino new file mode 100644 index 00000000..80f2fe64 --- /dev/null +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -0,0 +1,61 @@ +/** + * Smart Stepper support with SimpleFOClibrary + */ +#include + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, A2); + +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +int in1[2] = {5, 6}; +int in2[2] = {A4, 7}; +StepperDriver2PWM driver = StepperDriver2PWM(4, in1, 9, in2); + +// instantiate the commander +Commander command = Commander(SerialUSB); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // power supply voltage + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // comment out if not needed + motor.useMonitoring(SerialUSB); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "my motor"); + + SerialUSB.println(F("Motor ready.")); + SerialUSB.println(F("Set the target voltage using Serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + motor.loopFOC(); + + // Motion control function + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 7289ac79..4627efa9 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -3,8 +3,10 @@ // Stepper driver instance -// StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) -StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); +// StepperDriver2PWM(pwm1, in1, pwm2, in2, (en1, en2 optional)) +int in1[] = {4,5}; +int in2[] = {9,8}; +StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); // StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional)) // StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 47a49401..a9432b1d 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -1,13 +1,13 @@ #include "StepperDriver2PWM.h" -StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2){ +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){ // Pin initialization pwm1 = _pwm1; // phase 1 pwm pin number - dir1a = _in1a; // phase 1 INA pin number - dir1b = _in1b; // phase 1 INB pin number + dir1a = _in1[0]; // phase 1 INA pin number + dir1b = _in1[1]; // phase 1 INB pin number pwm2 = _pwm2; // phase 2 pwm pin number - dir2a = _in2a; // phase 2 INA pin number - dir2b = _in2b; // phase 2 INB pin number + dir2a = _in2[0]; // phase 2 INA pin number + dir2b = _in2[1]; // phase 2 INB pin number // enable_pin pin enable_pin1 = en1; @@ -94,10 +94,10 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); - if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW ); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); - if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); + if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index cab796d3..b349af06 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -16,15 +16,13 @@ class StepperDriver2PWM: public StepperDriver /** StepperMotor class constructor @param pwm1 PWM1 phase pwm pin - @param in1a IN1A phase dir pin - @param in1b IN1B phase dir pin + @param in1 IN1A phase dir pin @param pwm2 PWM2 phase pwm pin - @param in2a IN2A phase dir pin - @param in2b IN2B phase dir pin + @param in2 IN2A phase dir @param en1 enable pin phase 1 (optional input) @param en2 enable pin phase 2 (optional input) */ - StepperDriver2PWM(int pwm1, int in1a, int in1b, int pwm2, int in2a, int in2b, int en1 = NOT_SET, int en2 = NOT_SET); + StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET); /** StepperMotor class constructor From 98c10e6b96be818b4780025cb2ab75a651098c96 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 10:19:43 +0200 Subject: [PATCH 035/103] avoid compiling samd examples --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 2377cd61..9eea4f03 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control From 22d003c12135795d857d23645f2febe643d4fad3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Jun 2021 22:17:30 +0200 Subject: [PATCH 036/103] performance optimisation - don't return angle on updateSensor() --- src/BLDCMotor.cpp | 12 ++++++++---- src/common/base_classes/Sensor.cpp | 3 +-- src/common/base_classes/Sensor.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c0cb5b32..dda3d745 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - shaft_angle = sensor->updateSensor(); + sensor->updateSensor(); + shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -164,14 +165,16 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - float mid_angle = sensor->updateSensor(); + sensor->updateSensor(); + float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - float end_angle = sensor->updateSensor(); + sensor->updateSensor(); + float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved @@ -201,7 +204,8 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->updateSensor(), pole_pairs)); + sensor->updateSensor(); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index a3921998..1f6b649e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -4,14 +4,13 @@ // TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 -float Sensor::updateSensor() { +void Sensor::updateSensor() { float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; angle_prev = val; - return getAngle(); } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 78193158..47929cf3 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -95,7 +95,7 @@ class Sensor{ * * Returns the same value as getAngle() as its result */ - virtual float updateSensor(); + virtual void updateSensor(); /** * returns 0 if it does need search for absolute zero From 9cf18c27415cfe0d43da7ec56a8c2675c1e21196 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 21:09:00 +0200 Subject: [PATCH 037/103] doh! TCC0 has 6 output channels - fixed WO_associations table --- src/drivers/hardware_specific/samd51_mcu.cpp | 40 ++++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 08201b97..0fd339f2 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -31,8 +31,8 @@ struct wo_association WO_associations[] = { { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTB, 11, TC5_CH1, 1, TCC0_CH1, 5, TCC1_CH1, 1}, //? + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, //? { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, @@ -40,34 +40,34 @@ struct wo_association WO_associations[] = { { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, //? - { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, //? + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, //? + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, //? { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH1, 5, TCC1_CH1, 1}, //? - { PORTA, 12, TC2_CH0, 0, TCC0_CH2, 6, TCC1_CH2, 2}, - { PORTA, 13, TC2_CH1, 1, TCC0_CH3, 7, TCC1_CH3, 3}, + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, + { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? - { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH0, 4}, - { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH1, 5}, - { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH2, 6}, - { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH3, 7}, + { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, + { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, + { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, + { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH1, 7}, { PORTC, 16, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTC, 17, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTC, 18, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 { PORTC, 19, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, - { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, - { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH2, 6, NOT_ON_TIMER, 0}, - { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH3, 7, NOT_ON_TIMER, 0}, + { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, + { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH0, 6, NOT_ON_TIMER, 0}, + { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH1, 7, NOT_ON_TIMER, 0}, { PORTD, 20, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, { PORTD, 21, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 4}, - { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 5}, + { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH4, 4}, + { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH5, 5}, { PORTB, 18, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTB, 19, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTB, 20, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 @@ -89,8 +89,8 @@ struct wo_association WO_associations[] = { // PC24-PC28, PA27, RESET -> no TC/TCC peripherals { PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, { PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 6}, - { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 7}, + { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH0, 6}, + { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH1, 7}, // PC30, PC31 -> no TC/TCC peripherals { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, From a1149e187a55323b04d0af2712afd30b35b3deb1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 21:10:29 +0200 Subject: [PATCH 038/103] add enable_active_high setting to 3-PWM driver also --- src/drivers/BLDCDriver3PWM.cpp | 14 +++++++------- src/drivers/BLDCDriver3PWM.h | 1 + 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 7ccd09fa..a0f96599 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,9 +20,9 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, HIGH); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, HIGH); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, HIGH); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high); // set zero to PWM setPwm(0,0,0); } @@ -33,9 +33,9 @@ void BLDCDriver3PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, LOW); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, LOW); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, LOW); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high); } @@ -87,4 +87,4 @@ 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); -} \ No newline at end of file +} diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index cea4463e..d6a83590 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,6 +38,7 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number + bool enable_active_high = true; /** * Set phase voltages to the harware From 17dc7fb8135684f7a3deafbbafc62abff8819092 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 22:36:01 +0200 Subject: [PATCH 039/103] comments updated --- src/drivers/hardware_specific/samd51_mcu.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 0fd339f2..b59a43fc 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -31,8 +31,8 @@ struct wo_association WO_associations[] = { { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? - { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, @@ -40,18 +40,18 @@ struct wo_association WO_associations[] = { { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, //? - { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, //? + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? - { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, - { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? - { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? + { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, + { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, From a875e409a48563429165cd3882d0cd328ff03eed Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 10:50:34 +0200 Subject: [PATCH 040/103] esp32 2 or 3 adc lines automatic --- src/current_sense/LowsideCurrentSense.cpp | 2 + .../hardware_specific/esp32_mcu.cpp | 99 +++++++++++-------- 2 files changed, 59 insertions(+), 42 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 18aefd49..2ff2ce8b 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -23,6 +23,8 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int void LowsideCurrentSense::init(){ // configure ADC variables _configureADCLowSide(pinA,pinB,pinC); + // sync the driver + _driverSyncLowSide(); // calibrate zero offsets calibrateOffsets(); } diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index c880c56a..d4ac0587 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../drivers/hardware_api.h" #ifdef ESP_H @@ -14,15 +15,39 @@ #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 = adcRead(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); +} + + +/** + * 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 a1, a2, a3; // Current readings from internal current sensor amplifiers int _pinA, _pinB, _pinC; static void IRAM_ATTR isr_handler(void*); byte currentState = 1; -// 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 _readADCVoltageLowSide(const int pin){ @@ -39,71 +64,61 @@ float _readADCVoltageLowSide(const int pin){ void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; _pinB = pinB; - if( _isset(pinC) ) _pinC = pinC; + _pinC = pinC; pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - } void _driverSyncLowSide(){ + // 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 - //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 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 } // Read currents when interrupt is triggered static void IRAM_ATTR isr_handler(void*){ -// 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; + // // 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 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0; - // digitalWrite(14,HIGH); - if(mcpwm_intr_status_0 > 0 && currentState == 1){ - a1 = adcRead(_pinA); - // a2 = analogRead(_pinA); + switch (currentState) + { + case 1 : + if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); currentState = 2; - } - else if(mcpwm_intr_status_1 > 0 && currentState == 2){ - a2 = adcRead(_pinB); - // a3 = analogRead(_pinB); + 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; } - /* - else if(mcpwm_intr_status_2 > 0 && currentState == 3){ - a3 = adcRead(_pinC); - //a1 = analogRead(_pinA); - currentState = 1; - }*/ - // digitalWrite(14,LOW); + // 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 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; - //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; -} - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = adcRead(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); + if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } From 0aa327d46d6ef979e1d4c10058b5b5bbb33797fc Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:40:52 +0200 Subject: [PATCH 041/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 9eea4f03..b3116706 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -4,11 +4,35 @@ jobs: build: name: Test compiling runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno + - esp32:esp32:featheresp32:FlashFreq=80 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + + include: + - arduino-boards-fqbn: arduino:avr:uno + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + + - arduino-boards-fqbn: TM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + + - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false steps: - name: Checkout uses: actions/checkout@master - name: Compile all examples uses: ArminJo/arduino-test-compile@v1.0.0 with: - libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: PciManager + platform-url: ${{ matrix.platform-url }} + sketches-exclude: ${{ matrix.sketches-exclude }} From b966b5dc803c06df55f3c05983ffaa05eff13fc2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:41:48 +0200 Subject: [PATCH 042/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index b3116706..27e5e149 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -16,7 +16,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: TM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, From 9d8bd1027d18db772b19b9ad44471110fa16c8e6 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:46:34 +0200 Subject: [PATCH 043/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 27e5e149..5a48d128 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -33,6 +33,6 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: PciManager + libraries: PciManager platform-url: ${{ matrix.platform-url }} sketches-exclude: ${{ matrix.sketches-exclude }} From 265c4d1042017052ff560a8c4ffacbf9d276d98a Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:48:31 +0200 Subject: [PATCH 044/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5a48d128..56f595ed 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,20 +9,20 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - - esp32:esp32:featheresp32:FlashFreq=80 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + #- esp32:esp32:featheresp32:FlashFreq=80 + #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, - - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + # - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + # platform-url: https://dl.espressif.com/dl/package_esp32_index.json + # sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # Do not cancel all jobs / architectures if one job fails fail-fast: false From fecf077333893e1b4da3fd1fddb0b58009517914 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:00:08 +0200 Subject: [PATCH 045/103] 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 56f595ed..25223570 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -30,9 +30,9 @@ jobs: - name: Checkout uses: actions/checkout@master - name: Compile all examples - uses: ArminJo/arduino-test-compile@v1.0.0 + uses: ArminJo/arduino-test-compile@master with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - libraries: PciManager + required-libraries: PciManager platform-url: ${{ matrix.platform-url }} sketches-exclude: ${{ matrix.sketches-exclude }} From 075925d15370d78666d2b753cd6275652cb81826 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:09:19 +0200 Subject: [PATCH 046/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 25223570..6ed2829d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,13 +9,19 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - #- esp32:esp32:featheresp32:FlashFreq=80 + - arduono:sam:due +# - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno + sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + - arduino-boards-fqbn: arduino:sam:uno + sketch-names: single_full_control_example.ino + + # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, @@ -35,4 +41,5 @@ jobs: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} required-libraries: PciManager platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} sketches-exclude: ${{ matrix.sketches-exclude }} From 695ba4622d85b716018cff763f302a8c80d04b11 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:10:22 +0200 Subject: [PATCH 047/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6ed2829d..db9dfc03 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -18,7 +18,7 @@ jobs: sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:uno + - arduino-boards-fqbn: arduino:sam:due sketch-names: single_full_control_example.ino From 1a437171db83a813f27820586291c4af5acdd864 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:24:02 +0200 Subject: [PATCH 048/103] 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 db9dfc03..f294fa7e 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,7 +9,7 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - - arduono:sam:due + - arduino:sam:arduino_due_x # - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 @@ -18,7 +18,7 @@ jobs: sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:due + - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino From 221e48cfbaa5d73957a767aa8e4ea1e389d9394e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:27:44 +0200 Subject: [PATCH 049/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index f294fa7e..e31384e3 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,25 +10,27 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x -# - esp32:esp32:featheresp32:FlashFreq=80 + - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno sketch-names: '**.ino' + required-libraries: PciManager sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino - + + - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, - # - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - # platform-url: https://dl.espressif.com/dl/package_esp32_index.json - # sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + # Do not cancel all jobs / architectures if one job fails fail-fast: false @@ -39,7 +41,7 @@ jobs: uses: ArminJo/arduino-test-compile@master with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: PciManager + required-libraries: ${{ matrix.required-libraries }} platform-url: ${{ matrix.platform-url }} sketch-names: ${{ matrix.sketch-names }} sketches-exclude: ${{ matrix.sketches-exclude }} From 0bb5256467abb041985c1f3a7da5f762733ab406 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:28:09 +0200 Subject: [PATCH 050/103] 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 e31384e3..25bdb812 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -23,8 +23,8 @@ jobs: sketch-names: single_full_control_example.ino - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json From d157c86711614ba93d6c3f81a6bed540533dbdea Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:33:03 +0200 Subject: [PATCH 051/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 25bdb812..69131460 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,7 +11,7 @@ jobs: - arduino:avr:uno - arduino:sam:arduino_due_x - esp32:esp32:featheresp32:FlashFreq=80 - #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno @@ -22,13 +22,13 @@ jobs: - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + 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, magnetic_sensor_spi_alt_example.ino From 2ea35dd491850b1da83a29d69fd46f3d0bf38c11 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:39:37 +0200 Subject: [PATCH 052/103] FIX esp32 error --- .../encoder/esp32_position_control/esp32_position_control.ino | 3 --- .../esp32_position_control/esp32_position_control.ino | 3 --- 2 files changed, 6 deletions(-) diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 3943017b..f6dac940 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -85,9 +85,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 77cfeeed..d66b5e16 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -88,9 +88,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function From 07f842e24d6d7c3b17941cf8e1f15402d531b624 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:41:10 +0200 Subject: [PATCH 053/103] FIX bluepill examples --- .../bluepill_position_control/bluepill_position_control.ino | 2 +- .../bluepill_position_control/bluepill_position_control.ino | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index d0768fd2..be853f0e 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -89,7 +89,7 @@ void setup() { command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); - Serial.println(F(("Set the target angle using serial terminal:")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 1b47170d..e27f1e7a 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -91,9 +91,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function From 02616843024aafdf58a195bf80ad3a1c5cce5fad Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:41:51 +0200 Subject: [PATCH 054/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 69131460..8e4f665b 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,8 +10,9 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x - - esp32:esp32:featheresp32:FlashFreq=80 + - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 +# - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE include: - arduino-boards-fqbn: arduino:avr:uno @@ -30,6 +31,10 @@ jobs: 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, magnetic_sensor_spi_alt_example.ino + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + 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, magnetic_sensor_spi_alt_example.ino + # Do not cancel all jobs / architectures if one job fails From 999d000d532845e1ca1d93e027bf58a32f4058c1 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:46:32 +0200 Subject: [PATCH 055/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 8e4f665b..e04aa980 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,9 +10,10 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x + - arduino:samd:nano_33_iot - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 -# - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE include: - arduino-boards-fqbn: arduino:avr:uno @@ -22,6 +23,9 @@ jobs: - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:samd:nano_33_iot + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json @@ -31,9 +35,9 @@ jobs: 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, magnetic_sensor_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE 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, magnetic_sensor_spi_alt_example.ino + sketch-names: single_full_control_example.ino From 5c4c07586f7a4ecc1dbcd420ad709967de3d3549 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:50:38 +0200 Subject: [PATCH 056/103] FIX nano33 example --- .../nano33IoT_velocity_control/nano33IoT_velocity_control.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index 2c9baaef..70ec28b0 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -6,7 +6,6 @@ #include "Arduino.h" #include #include -#include // this is for an AS5048B absolute magnetic encoder on I2C address 0x41 MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8); From 954545b748d35fff9eaf0e4611106074c9546c47 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:52:13 +0200 Subject: [PATCH 057/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index e04aa980..d14dfaab 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -14,6 +14,7 @@ jobs: - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - arduino:mbed_rp2040:pico include: - arduino-boards-fqbn: arduino:avr:uno @@ -26,6 +27,9 @@ jobs: - arduino-boards-fqbn: arduino:samd:nano_33_iot sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico + sketch-names: open_loop_position_example.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json From dde0ef31431cb552413f8b7e574043c925e51ac2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:16:31 +0200 Subject: [PATCH 058/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index d14dfaab..c24ce6f0 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -15,6 +15,7 @@ jobs: - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - arduino:mbed_rp2040:pico + - adafruit:samd:adafruit_metro_m4 include: - arduino-boards-fqbn: arduino:avr:uno @@ -30,6 +31,10 @@ jobs: - arduino-boards-fqbn: arduino:mbed_rp2040:pico sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 + 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:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json From c89ae6bffea9e723f40a49cb7f81e4c710a25b35 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:19:12 +0200 Subject: [PATCH 059/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c24ce6f0..c3d269c3 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -8,43 +8,43 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:avr:uno - - arduino:sam:arduino_due_x - - arduino:samd:nano_33_iot - - esp32:esp32:esp32doit-devkit-v1 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - - arduino:mbed_rp2040:pico - - adafruit:samd:adafruit_metro_m4 + - arduino:avr:uno # arudino uno + - 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 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - arduino:mbed_rp2040:pico # rpi pico include: - - arduino-boards-fqbn: arduino:avr:uno + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:arduino_due_x + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: arduino:samd:nano_33_iot + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - - arduino-boards-fqbn: arduino:mbed_rp2040:pico + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example sketch-names: open_loop_position_example.ino - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://dl.espressif.com/dl/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - 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, magnetic_sensor_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: single_full_control_example.ino From 88d383b6eae0eb9fd6446fa15d91750384beb9ac Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 13:29:19 +0200 Subject: [PATCH 060/103] FEAT added low-side esp32 BRV8302 example --- .../esp32_current_control_low_side.ino | 164 ++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino 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 new file mode 100644 index 00000000..202aa337 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -0,0 +1,164 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A 21 +#define INH_B 19 +#define INH_C 18 + +#define EN_GATE 5 +#define M_PWM 25 +#define M_OC 26 +#define OC_ADJ 12 +#define OC_GAIN 14 + +#define IOUTA 34 +#define IOUTB 35 +#define IOUTC 32 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(22, 23, 500); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.pwm_frequency = 15000; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 1000; + + // initialise motor + motor.init(); + + cs.init(); + cs.driverSync(&driver); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file From 6267e797b0185ea4ddebe23670a68ef33559d432 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:31:12 +0200 Subject: [PATCH 061/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c3d269c3..b8eedae1 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -38,7 +38,7 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.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 From 5f64df651c37f43b7fc60df6991937cc77956e37 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:42:54 +0200 Subject: [PATCH 062/103] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index b8eedae1..07bb15b1 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,7 +21,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 1b11bd422ead5b149aa6635c0976cf025ef69991 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Thu, 1 Jul 2021 16:47:33 +0200 Subject: [PATCH 063/103] Prevent unintended float-to-double casts --- src/BLDCMotor.cpp | 6 +++--- src/common/foc_utils.cpp | 4 ++-- src/common/foc_utils.h | 4 ++-- src/common/pid.cpp | 2 +- src/current_sense/InlineCurrentSense.cpp | 14 +++++++------- src/current_sense/InlineCurrentSense.h | 12 ++++++------ src/current_sense/LowsideCurrentSense.h | 12 ++++++------ src/drivers/BLDCDriver3PWM.cpp | 6 +++--- src/drivers/StepperDriver2PWM.cpp | 4 ++-- src/drivers/StepperDriver4PWM.cpp | 8 ++++---- src/drivers/hardware_specific/stm32_mcu.cpp | 2 +- 11 files changed, 37 insertions(+), 37 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 93fcee17..db713edf 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -458,8 +458,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { center = driver->voltage_limit/2; // Clarke transform Ua = Ualpha + center; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + center; + Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; + Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); @@ -502,7 +502,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (modulation_centered) { diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 58ac36e5..11a1810f 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -23,11 +23,11 @@ float _sin(float a){ }else if(a < _3PI_2){ // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873f*a)]; // int array optimized + return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873f*a)]; // int array optimized + return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e58fbaa0..75bce025 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -5,7 +5,7 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) -#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) @@ -25,7 +25,7 @@ #define _3PI_2 4.71238898038f #define _PI_6 0.52359877559f -#define NOT_SET -12345.0 +#define NOT_SET -12345.0f #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 diff --git a/src/common/pid.cpp b/src/common/pid.cpp index c887c35b..6472c0f4 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -28,7 +28,7 @@ float PIDController::operator() (float error){ float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) - float integral = integral_prev + I*Ts*0.5*(error + error_prev); + float integral = integral_prev + I*Ts*0.5f*(error + error_prev); // antiwindup - limit the output integral = _constrain(integral, -limit, limit); // Discrete derivation diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 43e78b9f..bc3bc4c6 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -80,9 +80,9 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -117,9 +117,9 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -155,7 +155,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 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.0; + c.c = (c.c+c1.c)/50.0f; } driver->setPwm(0, 0, 0); gain_c *= _sign(c.c); diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 615c4733..748d3f44 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -48,17 +48,17 @@ class InlineCurrentSense: public CurrentSense{ int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio /** * Function finding zero offsets of the ADC */ void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + 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) }; diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 9618b572..057aa623 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -49,17 +49,17 @@ class LowsideCurrentSense: public CurrentSense{ int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio /** * Function finding zero offsets of the ADC */ void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + 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) }; diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 7ccd09fa..9ce089b8 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -75,9 +75,9 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver - Ua = _constrain(Ua, 0.0, voltage_limit); - Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Ua = _constrain(Ua, 0.0f, voltage_limit); + Ub = _constrain(Ub, 0.0f, voltage_limit); + Uc = _constrain(Uc, 0.0f, voltage_limit); // calculate duty cycle // limited in [0,1] dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a9432b1d..7cdd5045 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -89,8 +89,8 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); - duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); + duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); + duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index cfcb7aef..bf2681f6 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -66,14 +66,14 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + 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); } \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index b0ac00e7..fc7cf98e 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,7 +4,7 @@ #if defined(_STM32_DEF_) // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_RANGE 4095// 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz From edce7d4d53880d4faf37533321e3eeb955918138 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:08:34 +0200 Subject: [PATCH 064/103] some refactoring and bug fixng --- .../alignment_and_cogging_test.ino | 2 + .../find_pole_pairs_number.ino | 2 + .../find_pole_pairs_number.ino | 4 +- .../encoder_example/encoder_example.ino | 4 ++ .../encoder_software_interrupts_example.ino | 4 ++ .../hall_sensor_example.ino | 3 ++ ...all_sensors_software_interrupt_example.ino | 3 ++ .../find_raw_min_max/find_raw_min_max.ino | 5 ++ .../magnetic_sensor_analog_example.ino} | 5 ++ .../esp32_i2c_dual_bus_example.ino | 7 +++ .../stm32_i2c_dual_bus_example.ino | 7 +++ .../magnetic_sensor_i2c_example.ino | 6 +++ .../find_raw_min_max/find_raw_min_max.ino | 5 ++ .../magnetic_sensor_pwm_example.ino} | 5 ++ ...magnetic_sensor_pwm_software_interrupt.ino | 5 ++ .../magnetic_sensor_spi_alt_example.ino | 5 ++ .../magnetic_sensor_spi_example.ino | 5 ++ keywords.txt | 3 ++ src/BLDCMotor.cpp | 53 +++++++++---------- src/StepperMotor.cpp | 41 +++++++++++--- src/common/base_classes/FOCMotor.cpp | 4 +- src/common/base_classes/Sensor.cpp | 6 +-- src/common/base_classes/Sensor.h | 30 +++++------ src/sensors/Encoder.cpp | 7 +-- src/sensors/Encoder.h | 2 +- src/sensors/HallSensor.cpp | 7 +-- src/sensors/HallSensor.h | 2 +- 27 files changed, 169 insertions(+), 63 deletions(-) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_analog_example => magnetic_sensor_analog}/find_raw_min_max/find_raw_min_max.ino (89%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino => magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino} (83%) rename 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 (77%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_i2c}/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino (74%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_i2c}/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino (81%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example => magnetic_sensor_pwm}/find_raw_min_max/find_raw_min_max.ino (85%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino => magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino} (83%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example => magnetic_sensor_pwm}/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino (85%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_spi}/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino (75%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_spi}/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino (76%) 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 ca254a5f..3fcd7b30 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 @@ -21,6 +21,7 @@ void testAlignmentAndCogging(int direction) { motor.move(0); _delay(200); + sensor.update(); float initialAngle = sensor.getAngle(); const int shaft_rotation = 720; // 720 deg test - useful to see repeating cog pattern @@ -40,6 +41,7 @@ void testAlignmentAndCogging(int direction) { _delay(5); // measure + sensor.update(); float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; 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 e6187921..c5dfbf43 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 @@ -64,6 +64,7 @@ void setup() { motor.move(0); _delay(1000); // read the encoder angle + encoder.update(); float angle_begin = encoder.getAngle(); _delay(50); @@ -75,6 +76,7 @@ void setup() { } _delay(1000); // read the encoder value for 180 + encoder.update(); float angle_end = encoder.getAngle(); _delay(50); // turn off the motor 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 c2785c55..0b07c772 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 @@ -63,6 +63,7 @@ void setup() { motor.move(0); _delay(1000); // read the sensor angle + sensor.update(); float angle_begin = sensor.getAngle(); _delay(50); @@ -70,11 +71,12 @@ void setup() { float motor_angle = 0; while(motor_angle <= pp_search_angle){ motor_angle += 0.01f; - sensor.getAngle(); // keep track of the overflow + sensor.update(); // keep track of the overflow motor.move(motor_angle); } _delay(1000); // read the sensor value for 180 + sensor.update(); float angle_end = sensor.getAngle(); _delay(50); // turn off the motor diff --git a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino index 66aca6d8..f105f5b0 100644 --- a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino @@ -33,6 +33,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino index a979d9a0..ebf2dd41 100644 --- a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino @@ -46,6 +46,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index 6c19a2e9..ef2e504c 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -37,6 +37,9 @@ void setup() { } 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"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino index 523af03f..238eb2c4 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino @@ -49,6 +49,9 @@ void setup() { } 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"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino similarity index 89% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino index e45b0278..34e68a1b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino @@ -33,6 +33,11 @@ int max_count = 0; int min_count = 100000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.raw_count > max_count) max_count = sensor.raw_count; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino index 40d47ba8..2496151b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino @@ -25,6 +25,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/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 similarity index 77% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino rename to 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 c781847f..c930437b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/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 @@ -25,6 +25,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino similarity index 74% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino index 019e088c..08fb145d 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino @@ -24,6 +24,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino similarity index 81% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino index 30afc90c..4e060c0a 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino @@ -30,6 +30,12 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); + // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino index a88b7186..ac9bf044 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino @@ -27,6 +27,11 @@ int max_pulse= 0; int min_pulse = 10000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino index 15a1f557..6ae0a3e9 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino @@ -26,6 +26,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino index 6b19a9fa..10dc8a6b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -32,6 +32,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino similarity index 75% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino index 249837e6..713a19b1 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino similarity index 76% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino index 319c3741..048620ac 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/keywords.txt b/keywords.txt index 1d8156f5..b47c18fd 100644 --- a/keywords.txt +++ b/keywords.txt @@ -77,6 +77,9 @@ ISR KEYWORD2 getVelocity KEYWORD2 setPhaseVoltage KEYWORD2 getAngle KEYWORD2 +getMechanicalAngle KEYWORD2 +getSensorAngle KEYWORD2 +update KEYWORD2 needsSearch KEYWORD2 useMonitoring KEYWORD2 angleOpenloop KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8e9afe2c..7a25bdd8 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - sensor->updateSensor(); + sensor->update(); shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); @@ -165,7 +165,7 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - sensor->updateSensor(); + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { @@ -173,7 +173,7 @@ int BLDCMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - sensor->updateSensor(); + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); @@ -204,8 +204,11 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - sensor->updateSensor(); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -234,7 +237,7 @@ int BLDCMotor::absoluteZeroSearch() { angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->updateSensor(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -254,23 +257,17 @@ int BLDCMotor::absoluteZeroSearch() { void BLDCMotor::loopFOC() { // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track // of full rotations otherwise. - if (sensor) sensor->updateSensor(); + if (sensor) sensor->update(); // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float - // For this reason it is NOT precise when the angles become large. - // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem - // when switching to a 2-component representation. - shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated - // if disabled do nothing if(!enabled) return; - // electrical angle - need shaftAngle to be called first - // TODO sensor precision: this will have precision issues because the shaft_angle does... + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); switch (torque_controller) { @@ -318,7 +315,16 @@ void BLDCMotor::move(float new_target) { // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; - // get angular velocity - sensor precision: this value is numerically precise. It is filtered by Velocity LPF, and downsamling (depending on sensor) + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing @@ -527,7 +533,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (modulation_centered) { - T0 = 1 - T1 - T2; //modulation_centered around driver->voltage_limit/2 + T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 } // calculate the duty cycles(times) @@ -595,8 +601,6 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // quick fix for strange cases (micros overflow + timestamp not defined) if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - // sensor precision: this calculation is numerically precise since we re-normalize - // the shaft-angle to the range 0-2PI // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes @@ -606,8 +610,6 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; - // sensor precision: this calculation is numerically precise, since shaft_angle is - // in the range 0-2PI // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -646,11 +648,8 @@ float BLDCMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - // TODO sensor precision: this calculation is not numerically precise. The angle is not constrained, - // and can grow large. It first gets multiplied by the number of pole-pairs (in _electrocalAngle()), - // and then gets normalized to 0-2PI (before it gets used in setPhaseVoltage()). At large angles, - // the precision can mean the range 0-2PI is not well represented at the end of that calculation. - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + // sensor precision: this calculation is OK due to the normalisation + setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index ca0716b7..2acd6f11 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -99,6 +99,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update + sensor->update(); shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); @@ -132,6 +133,7 @@ int StepperMotor::alignSensor() { _delay(2); } // take and angle in the middle + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { @@ -139,6 +141,7 @@ int StepperMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); @@ -169,7 +172,11 @@ int StepperMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -197,7 +204,7 @@ int StepperMotor::absoluteZeroSearch() { angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -216,6 +223,11 @@ int StepperMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void StepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; // shaft angle @@ -224,6 +236,9 @@ void StepperMotor::loopFOC() { // if disabled do nothing if(!enabled) return; + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); // set the phase voltage - FOC heart function :) @@ -236,13 +251,25 @@ void StepperMotor::loopFOC() { // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { - // get angular velocity - shaft_velocity = shaftVelocity(); - // if disabled do nothing - if(!enabled) return; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + // set internal target variable if(_isset(new_target) ) target = new_target; // choose control loop @@ -370,7 +397,7 @@ float StepperMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index eae18e67..579f07ee 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -66,7 +66,9 @@ float FOCMotor::shaftVelocity() { } float FOCMotor::electricalAngle(){ - return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); + // if no sensor linked return previous value ( for open loop ) + if(!sensor) return electrical_angle; + return sensor_direction * _normalizeAngle(sensor->getMechanicalAngle() * pole_pairs - zero_electric_angle); } /** diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 1f6b649e..a5ad1b6d 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -4,7 +4,7 @@ // TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 -void Sensor::updateSensor() { +void Sensor::update() { float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; @@ -14,7 +14,7 @@ void Sensor::updateSensor() { } - /** get current angular velocity (rad/s)*/ + /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; @@ -43,7 +43,7 @@ void Sensor::init() { } -float Sensor::getShaftAngle() { +float Sensor::getMechanicalAngle() { return angle_prev; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 47929cf3..3a946bc7 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -32,7 +32,7 @@ enum Pullup{ * method. getAngle() returns a float value, in radians, representing the current shaft angle in the * range 0 to 2*PI (one full turn). * - * To function correctly, the sensor class updateSensor() method has to be called sufficiently quickly. Normally, + * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally, * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly * enough, both for correct motor and sensor operation. * @@ -44,16 +44,16 @@ enum Pullup{ class Sensor{ public: /** - * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with - * the hardware. Base implementation uses the values returned by updateSensor() so that - * the same values are returned until updateSensor() is called again. + * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with + * the hardware. Base implementation uses the values returned by update() so that + * the same values are returned until update() is called again. */ - virtual float getShaftAngle(); + virtual float getMechanicalAngle(); /** * Get current position (in rad) including full rotations and shaft angle. - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. * Note that this value has limited precision as the number of rotations increases, * because the limited precision of float can't capture the large angle of the full * rotations and the small angle of the shaft angle at the same time. @@ -63,23 +63,23 @@ class Sensor{ /** * On architectures supporting it, this will return a double precision position value, * which should have improved precision for large position values. - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. */ virtual double getPreciseAngle(); /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values - * returned by updateSensor() so that it only makes sense to call this if updateSensor() + * returned by update() so that it only makes sense to call this if update() * has been called in the meantime. */ virtual float getVelocity(); /** * Get the number of full rotations - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. */ virtual int32_t getFullRotations(); @@ -92,10 +92,8 @@ class Sensor{ * rotations are not "missed" due to infrequent polling. * Override in subclasses if alternative behaviours are required for your * sensor hardware. - * - * Returns the same value as getAngle() as its result */ - virtual void updateSensor(); + virtual void update(); /** * returns 0 if it does need search for absolute zero @@ -110,7 +108,7 @@ class Sensor{ * * This method is pure virtual and must be implemented in subclasses. * Calling this method directly does not update the base-class internal fields. - * Use updateSensor() when calling from outside code. + * Use update() when calling from outside code. */ virtual float getSensorAngle()=0; /** diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 376c8092..9512e20a 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -102,10 +102,11 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getShaftAngle(); + return getAngle(); } -float Encoder::getShaftAngle(){ - return _2PI * (pulse_counter % (int)cpr); +// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost +float Encoder::getMechanicalAngle(){ + return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); } float Encoder::getAngle(){ diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index c3cab450..b567fa4e 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,7 +61,7 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getShaftAngle() override; + float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; float getAngle() override; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index d6f24398..467ba9e7 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -95,16 +95,17 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { float HallSensor::getSensorAngle() { - return getShaftAngle(); + return getAngle(); } /* Shaft angle calculation + TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getShaftAngle() { - return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; +float HallSensor::getMechanicalAngle() { + return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; } /* diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index ef991b77..9e80ed2b 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -55,7 +55,7 @@ class HallSensor: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getShaftAngle() override; + float getMechanicalAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; From 8b3046f083b6c7e7cd76a8c9f43588e586f3797e Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:09:26 +0200 Subject: [PATCH 065/103] quick readme update --- README.md | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index b9042658..1f83f36c 100644 --- a/README.md +++ b/README.md @@ -16,24 +16,8 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes be: SimpleFOClibrary v2.1.1 -> - bugfixes commander -> - bugfix `voltage_limit` when provided `phase_resistance` -> - bugfix `hall_sensor` examples -> - added examples fot the PowerShield -> - added initial support for `MagneticSensorPWM` -> - SAMD51 support -> - **initial support for Raspberry pi Pico** -> - added examples to find the raw max and min of the analog and pwm sensor -> - extension of the `Commander` interface -> - added pwm centering option `WC` -> - added pwm modulation cmd `WT` -> - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler -> - issue #62: motor.shaft_angle setting in the motor.initFOC() -> - issue #76: esp32 division by zero -> - issue #46: commander end of line character - by @maxlem -> - [community fix](https://community.simplefoc.com/t/as5600-dead-spot-around-0/208) AS5600 register value -> - renamed `Pullup::EXTERN` and `Pullup::INTERN` to `Pullup::USE_EXTERN` and `Pullup::USE_INTERN` +##### Release notes be: SimpleFOClibrary v2.1.2 +> - Sensor floating point error bugfux #83 ## Arduino *SimpleFOClibrary* v2.1 From f204ca5d30eebfdf57758aba4ec63c53ff870c7c Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:21:31 +0200 Subject: [PATCH 066/103] memory constraint arudino uno --- .../single_full_control_example/single_full_control_example.ino | 2 +- .../double_full_control_example/double_full_control_example.ino | 2 +- .../single_full_control_example/single_full_control_example.ino | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 0b3075d3..dad3e18b 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 @@ -77,7 +77,7 @@ void setup() { command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); + Serial.println("Motor commands sketch | Initial motion control > torque/current : target 0Amps."); _delay(1000); } 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 3399d2fa..6024321b 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 @@ -115,7 +115,7 @@ void setup() { command.add('B', doMotor2, "motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Double motor sketch ready.")); + Serial.println("Double motor sketch ready."); _delay(1000); } 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 67d1d869..0762d940 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 @@ -76,7 +76,7 @@ void setup() { command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); _delay(1000); } From 7ed3868488fb51ee68bc0ba10b5ec1d872e7ce50 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:40:09 +0200 Subject: [PATCH 067/103] Arduino UNO memeory shrinking --- README.md | 2 +- .../single_full_control_example.ino | 4 +++- .../double_full_control_example.ino | 12 ++++++++---- .../single_full_control_example.ino | 8 +++++--- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 1f83f36c..25983189 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes be: SimpleFOClibrary v2.1.2 +##### Release notes: SimpleFOClibrary v2.1.2 > - Sensor floating point error bugfux #83 ## Arduino *SimpleFOClibrary* v2.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 dad3e18b..34c0883a 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 @@ -17,6 +17,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); // commander communication instance Commander command = Commander(Serial); void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { @@ -75,9 +76,10 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); + command.add('T', doTarget, "target"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/current : target 0Amps."); + Serial.println("Motor ready."); _delay(1000); } 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 6024321b..837602ab 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 @@ -32,8 +32,10 @@ InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); // commander communication instance Commander command = Commander(Serial); -void doMotor1(char* cmd){ command.motor(&motor1, cmd); } -void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +// void doMotor1(char* cmd){ command.motor(&motor1, cmd); } +// void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +void doTarget1(char* cmd){ command.scalar(&motor1.target, cmd); } +void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { @@ -111,8 +113,10 @@ void setup() { motor2.target = 2; // subscribe motor to the commander - command.add('A', doMotor1, "motor 1"); - command.add('B', doMotor2, "motor 2"); + // command.add('A', doMotor1, "motor 1"); + // command.add('B', doMotor2, "motor 2"); + command.add('A', doTarget1, "target motor 1"); + command.add('B', doTarget2, "target motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println("Double motor sketch ready."); 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 0762d940..d6a00d60 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,8 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -73,8 +74,9 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('M', doMotor, "motor"); - + command.add('T', doTarget, "target"); + // command.add('M', doMotor, "motor"); + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); From 361d043fbb656d7eb7ddda46f3898fd86993ac0d Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:53:52 +0200 Subject: [PATCH 068/103] v3 memory optim for Arduino UNO --- .../single_full_control_example.ino | 4 ++-- .../double_full_control_example.ino | 6 +++--- .../single_full_control_example.ino | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) 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 34c0883a..dfad6e3a 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 @@ -16,7 +16,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { @@ -75,7 +75,7 @@ void setup() { motor.target = 0; // subscribe motor to the commander - command.add('M', doMotor, "motor"); + // command.add('M', doMotor, "motor"); command.add('T', doTarget, "target"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) 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 837602ab..259b23ee 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 @@ -115,11 +115,11 @@ void setup() { // subscribe motor to the commander // command.add('A', doMotor1, "motor 1"); // command.add('B', doMotor2, "motor 2"); - command.add('A', doTarget1, "target motor 1"); - command.add('B', doTarget2, "target motor 2"); + command.add('A', doTarget1, "target 1"); + command.add('B', doTarget2, "target 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Double motor sketch ready."); + Serial.println("Motors ready."); _delay(1000); } 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 d6a00d60..2d6c3ca6 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 @@ -78,7 +78,7 @@ void setup() { // command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + Serial.println("Motor ready."); _delay(1000); } From 3d9fee867d41cdfe968654567bfff48d7bac9085 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Tue, 3 Aug 2021 08:14:14 +0200 Subject: [PATCH 069/103] Rename hall_sensors_software_interrupt_example.ino to hall_sensors_software_interrupts_example.ino --- ...t_example.ino => hall_sensors_software_interrupts_example.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/{hall_sensors_software_interrupt_example.ino => hall_sensors_software_interrupts_example.ino} (100%) diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino similarity index 100% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino From b7266ff431ca82974af07836fa0634dc871ab339 Mon Sep 17 00:00:00 2001 From: candyriver <37467728+candyriver@users.noreply.github.com> Date: Sat, 21 Aug 2021 14:35:24 +0800 Subject: [PATCH 070/103] Create atmega32u4_mcu.cpp add support for atmega32u4 --- .../hardware_specific/atmega32u4_mcu.cpp | 130 ++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 src/drivers/hardware_specific/atmega32u4_mcu.cpp diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp new file mode 100644 index 00000000..79cc9169 --- /dev/null +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -0,0 +1,130 @@ + +#include "../hardware_api.h" + +#if defined(__AVR_ATmega32U4__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html + if ( pin == 6 || pin == 13 ) { + TCCR4A=0x82; + TCCR4B=2; // TCCR4B=2 32khz; TCCR4B=1 64khz; TTCR4B=3 16khz + TCCR4C|=0x09; + TCCR4D=0; + } + if ( pin == 5 ){ + TCCR3B=1; + } + + if (pin == 9 || pin == 10 ){ + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + } + + if (pin == 3 || pin == 11 ) { + TCCR0B = ((TCCR0B & 0b11111000) | 0x01); // 62kHZ + //TCCR0B = ((TCCR0B & 0b11111000) | 0x02); // 7.8 khz + } +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// 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) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// 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){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*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){ + // 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); +} + +// 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) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// 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){ + // 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); +} + + + +// Configuring PWM frequency, resolution and alignment +// - 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) { + return -1; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - 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); +} + +#endif From d226061b325f5c86063eadf45d1a914db4ee059a Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 10:37:44 +0200 Subject: [PATCH 071/103] added support for leonardo #108 and some cases for stm32l0x achitecture --- src/common/time_utils.cpp | 4 +- .../hardware_specific/atmega2560_mcu.cpp | 2 +- .../hardware_specific/atmega32u4_mcu.cpp | 158 ++++++++++++++++++ src/drivers/hardware_specific/stm32_mcu.cpp | 27 ++- 4 files changed, 184 insertions(+), 7 deletions(-) create mode 100644 src/drivers/hardware_specific/atmega32u4_mcu.cpp diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp index 65ebad22..2acb47a3 100644 --- a/src/common/time_utils.cpp +++ b/src/common/time_utils.cpp @@ -3,7 +3,7 @@ // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips // use while instad of delay, // due to wrong measurement based on changed timer0 @@ -19,7 +19,7 @@ void _delay(unsigned long ms){ // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips //return the value based on the prescaler if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 3d50dc45..2d3c0305 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -7,7 +7,7 @@ void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 // https://forum.arduino.cc/index.php?topic=72092.0 - if (pin == 13 || pin == 4 ) { + if (pin == 13 || pin == 4 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp new file mode 100644 index 00000000..1442a434 --- /dev/null +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -0,0 +1,158 @@ + +#include "../hardware_api.h" + +#if defined(__AVR_ATmega32U4__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html + if (pin == 3 || pin == 11 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + else if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 5 ) + TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1 + else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13 + else if (pin == 13) TCCR4C |= 0x09; // Activate channel D - pin 6 + } + +} + + +// 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) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// 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) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// 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){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*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){ + // 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); +} + +// 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) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// 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){ + // 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); +} + + + +// 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( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){ + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + // configure complementary pwm on low side + if(pinH == 13 ){ + TCCR4A = 0x82; // activate channel A - pin 13 + TCCR4C |= 0x0d; // Activate complementary channel D - pin 6 + }else { + TCCR4C |= 0x09; // Activate channel D - pin 6 + TCCR4A = 0xc2; // activate complementary channel A - pin 13 + } + }else{ + return -1; + } + return 0; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - 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); +} + +#endif diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index b0ac00e7..215ece87 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,7 +4,7 @@ #if defined(_STM32_DEF_) // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz @@ -70,10 +70,14 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCMode = TIM_OCMODE_PWM2; sConfigOC.Pulse = 100; sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#if defined(TIM_OCIDLESTATE_SET) + sConfig.OCIdleState = TIM_OCIDLESTATE_SET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); } @@ -121,6 +125,8 @@ 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) { + +#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface PinName uhPinName = digitalPinToPinName(pinA_h); PinName ulPinName = digitalPinToPinName(pinA_l); PinName vhPinName = digitalPinToPinName(pinB_h); @@ -157,6 +163,9 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->refresh(); HT->resume(); return HT; +#else + return nullptr; // return nothing +#endif } @@ -176,12 +185,22 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + +#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface + + if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +#else // the rest of stm32 boards + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else return _ERROR_6PWM; // might be error avoid configuration +#endif } From 9f689db1224aaf131958adf24957b57dc6554ac5 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:35:32 +0200 Subject: [PATCH 072/103] added initial support for portenta, intial support for esp8266 + leonardo + esp32 separation from esp8266 --- .../hardware_specific/esp32_adc_driver.cpp | 2 +- .../hardware_specific/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32_mcu.cpp | 2 +- .../hardware_specific/atmega32u4_mcu.cpp | 27 +- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- src/drivers/hardware_specific/esp8266_mcu.cpp | 77 +++ src/drivers/hardware_specific/generic_mcu.cpp | 2 +- .../hardware_specific/portenta_h7_mcu.cpp | 539 ++++++++++++++++++ 8 files changed, 641 insertions(+), 12 deletions(-) create mode 100644 src/drivers/hardware_specific/esp8266_mcu.cpp create mode 100644 src/drivers/hardware_specific/portenta_h7_mcu.cpp diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 01e1ca60..76ee54e9 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" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #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 a01eef19..df8166f0 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" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index d4ac0587..7b9ba448 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" #include "../../drivers/hardware_api.h" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 1442a434..fc782dfa 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -17,7 +17,7 @@ void _pinHighFrequency(const int pin){ TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1 else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer // PLL Configuration - PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode @@ -93,11 +93,8 @@ 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) { - +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); @@ -114,7 +111,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){ // PLL Configuration - PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode @@ -132,6 +129,22 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return 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) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + // 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 +} + // function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index c87e3b70..f6663d87 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) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp new file mode 100644 index 00000000..ccc31aa2 --- /dev/null +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -0,0 +1,77 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFreq(freq); +} + + +// 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) { + 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); +} + +// 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 || !_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); +} + +// 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 || !_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); +} + +// 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){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 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){ + // 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); +} + +// 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){ + // 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); +} + +#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 5cc8bc4b..f89c0562 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" // if the mcu doen't have defiend analogWrite -#if defined(ESP_H) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp new file mode 100644 index 00000000..74a66348 --- /dev/null +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -0,0 +1,539 @@ + +#include "../hardware_api.h" + +#if 1 //defined(TARGET_PORTENTA_H7) + +#include "pwmout_api.h" +#include "pinDefinitions.h" +#include "platform/mbed_critical.h" +#include "platform/mbed_power_mgmt.h" +#include "platform/mbed_assert.h" +#include "PeripheralPins.h" +#include "pwmout_device.h" + +// default pwm parameters +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +// // 6pwm parameters +// #define _HARDWARE_6PWM 1 +// #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; + + +/* Convert STM32 Cube HAL channel to LL channel */ +uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj) +{ +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1N; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2N; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4N; +#endif + default : /* Optional */ + return 0; + } + } else +#endif + { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3; + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4; + default : /* Optional */ + return 0; + } + } +} + + + +// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ +// return _pwm_init(obj, digitalPinToPinName(pin), frequency); +// } + +int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ + int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM); + int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM); + + const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function}; + + pwmout_init_direct(obj, &static_pinmap); + + TIM_HandleTypeDef TimHandle; + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + RCC_ClkInitTypeDef RCC_ClkInitStruct; + uint32_t PclkFreq = 0; + uint32_t APBxCLKDivider = RCC_HCLK_DIV1; + uint8_t i = 0; + + + __HAL_TIM_DISABLE(&TimHandle); + + // Get clock configuration + // Note: PclkFreq contains here the Latency (not used after) + HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq); + + /* Parse the pwm / apb mapping table to find the right entry */ + while (pwm_apb_map_table[i].pwm != obj->pwm) i++; + // sanity check + if (pwm_apb_map_table[i].pwm == 0) return -1; + + + if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) { + PclkFreq = HAL_RCC_GetPCLK1Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider; + } else { +#if !defined(PWMOUT_APB2_NOT_SUPPORTED) + PclkFreq = HAL_RCC_GetPCLK2Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider; +#endif + } + + long period_us = 500000.0/((float)frequency); + /* By default use, 1us as SW pre-scaler */ + obj->prescaler = 1; + // TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick + } + TimHandle.Init.Period = (period_us - 1); + + /* In case period or pre-scalers are out of range, loop-in to get valid values */ + while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) { + obj->prescaler = obj->prescaler * 2; + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1; + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1; + } + TimHandle.Init.Period = (period_us - 1) / obj->prescaler; + /* Period decreases and prescaler increases over loops, so check for + * possible out of range cases */ + if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) { + break; + } + } + + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned + + if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { + return -1; + } + + TIM_OC_InitTypeDef sConfig; + // Configure channels + sConfig.OCMode = TIM_OCMODE_PWM1; + sConfig.Pulse = obj->pulse / obj->prescaler; + sConfig.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfig.OCFastMode = TIM_OCFAST_DISABLE; +#if defined(TIM_OCIDLESTATE_RESET) + sConfig.OCIdleState = TIM_OCIDLESTATE_RESET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif + + int channel = 0; + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return -1; + } + + if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) { + // If channel is not enabled, proceed to channel configuration + if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) { + return -1; + } + } + + // Save for future use + obj->period = period_us; +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + HAL_TIMEx_PWMN_Start(&TimHandle, channel); + } else +#endif + { + HAL_TIM_PWM_Start(&TimHandle, channel); + } + + return 0; +} + +// setting pwm to hardware pin - instead analogWrite() +void _pwm_write(pwmout_t *obj, float value){ + TIM_HandleTypeDef TimHandle; + int channel = 0; + + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + + if (value < (float)0.0) { + value = 0.0; + } else if (value > (float)1.0) { + value = 1.0; + } + + obj->pulse = (uint32_t)((float)obj->period * value + 0.5); + + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return; + } + + // If channel already enabled, only update compare value to avoid glitch + __HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler); +} + +// init low side pin +// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +// { +// PinName pin = digitalPinToPinName(ulPin); +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); +// sConfigOC.OCMode = TIM_OCMODE_PWM2; +// sConfigOC.Pulse = 100; +// sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; +// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; +// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; +// sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; +// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); +// } +// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); +// HT->setOverflow(PWM_freq, HERTZ_FORMAT); +// HT->pause(); +// HT->refresh(); +// return HT; +// } + + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){ + TIM_HandleTypeDef TimHandle1, TimHandle2; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_DISABLE(&TimHandle4); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle4); +} + +// // 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) +// { +// PinName uhPinName = digitalPinToPinName(pinA_h); +// PinName ulPinName = digitalPinToPinName(pinA_l); +// PinName vhPinName = digitalPinToPinName(pinB_h); +// PinName vlPinName = digitalPinToPinName(pinB_l); +// PinName whPinName = digitalPinToPinName(pinC_h); +// PinName wlPinName = digitalPinToPinName(pinC_l); + +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); +// } +// 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); + +// // dead time is set in nanoseconds +// uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; +// uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); +// LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! +// LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + +// HT->pause(); +// HT->refresh(); +// HT->resume(); +// return HT; +// } + + +// // returns 0 if each pair of pwm channels has same channel +// // returns 1 all the channels belong to the same timer - hardware inverted channels +// // returns -1 if neither - avoid configuring - error!!! +// int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +// PinName nameAH = digitalPinToPinName(pinA_h); +// PinName nameAL = digitalPinToPinName(pinA_l); +// PinName nameBH = digitalPinToPinName(pinB_h); +// PinName nameBL = digitalPinToPinName(pinB_l); +// PinName nameCH = digitalPinToPinName(pinC_h); +// PinName nameCL = digitalPinToPinName(pinC_l); +// int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); +// int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); +// int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); +// int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); +// int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); +// int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); +// if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) +// return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer +// else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) +// return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer +// else +// return _ERROR_6PWM; // might be error avoid configuration +// } + + + +// 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) { + 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; + + 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); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + core_util_critical_section_exit(); + slot_index++; +} + + +// 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 || !_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; + 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); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + core_util_critical_section_exit(); + slot_index++; +} + +// 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 || !_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; + 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); + // 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])); + core_util_critical_section_exit(); + slot_index++; +} + +// 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; i Date: Wed, 25 Aug 2021 13:36:50 +0200 Subject: [PATCH 073/103] separation esp32 + esp8266 --- src/sensors/MagneticSensorSPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 6ba31d15..b7a9dd27 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -128,7 +128,7 @@ word MagneticSensorSPI::read(word angle_register){ spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); -#if defined( ESP_H ) // if ESP32 board +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) // if ESP32 board delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 From d518987373d11d0b9fec14d09558b397470f73a5 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:37:46 +0200 Subject: [PATCH 074/103] pwm frequency not set initially --- src/drivers/BLDCDriver6PWM.cpp | 1 + src/drivers/StepperDriver2PWM.cpp | 1 + src/drivers/StepperDriver4PWM.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 6039f69e..eaeb2cfa 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,6 +15,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; // dead zone initial - 2% dead_zone = 0.02f; diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a9432b1d..92092556 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -36,6 +36,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index cfcb7aef..f7918cc2 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -14,6 +14,7 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } From 9251630bfccf4ae8f171e8a5f63045790cb9f09b Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:46:42 +0200 Subject: [PATCH 075/103] remove default params from checkbus, encoder added empty update, bug in FOCMotor::electricalAngle --- src/BLDCMotor.cpp | 1 + src/common/base_classes/FOCMotor.cpp | 2 +- src/sensors/Encoder.cpp | 5 +++++ src/sensors/Encoder.h | 3 ++- src/sensors/MagneticSensorI2C.h | 2 +- 5 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 3ae98893..b0c84ba4 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -209,6 +209,7 @@ int BLDCMotor::alignSensor() { // get the current zero electric angle zero_electric_angle = 0; zero_electric_angle = electricalAngle(); + //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 579f07ee..41c0dfd2 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -68,7 +68,7 @@ float FOCMotor::shaftVelocity() { float FOCMotor::electricalAngle(){ // if no sensor linked return previous value ( for open loop ) if(!sensor) return electrical_angle; - return sensor_direction * _normalizeAngle(sensor->getMechanicalAngle() * pole_pairs - zero_electric_angle); + return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle ); } /** diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 9512e20a..7c164878 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -98,6 +98,11 @@ void Encoder::handleIndex() { } } + +void Encoder::update() { + // do nothing for Encoder +} + /* Shaft angle calculation */ diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index b567fa4e..3e775338 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -27,7 +27,7 @@ class Encoder: public Sensor{ Encoder(int encA, int encB , float ppr, int index = 0); /** encoder initialise pins */ - void init(); + void init() override; /** * function enabling hardware interrupts for the encoder channels with provided callback functions * if callback is not provided then the interrupt is not enabled @@ -67,6 +67,7 @@ class Encoder: public Sensor{ float getAngle() override; double getPreciseAngle() override; int32_t getFullRotations() override; + virtual void update() override; /** * returns 0 if it does need search for absolute zero diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 29bebee4..fa7ec96b 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -49,7 +49,7 @@ class MagneticSensorI2C: public Sensor{ float getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ - int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + int checkBus(byte sda_pin , byte scl_pin ); private: float cpr; //!< Maximum range of the magnetic sensor From 3a4ea20e5ab9c159a4205d50490d271c3a6304f1 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:49:56 +0200 Subject: [PATCH 076/103] update readme + library properties --- README.md | 7 ++++++- library.properties | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 25983189..fe7ab263 100644 --- a/README.md +++ b/README.md @@ -16,8 +16,13 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes: SimpleFOClibrary v2.1.2 +##### Release notes: SimpleFOClibrary v2.2 > - Sensor floating point error bugfux #83 +> - Support for portenta h7 board +> - Support for arduino leonardo #108 +> - Support for esp8266 +> - Low side current sensing support for esp32 +> - Restructured the generic code and simplified adding new mcus ## Arduino *SimpleFOClibrary* v2.1 diff --git a/library.properties b/library.properties index 0d89185d..89540636 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.2 +version=2.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 63d56321d33e4dd33600590fd3a1ed512041d3a2 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:51:22 +0200 Subject: [PATCH 077/103] forgotten pwm_frequency default --- src/drivers/BLDCDriver3PWM.cpp | 1 + src/drivers/StepperDriver2PWM.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 88d5ab95..bc239875 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -14,6 +14,7 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 8881709f..a0e61ca0 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -16,6 +16,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } From a71072945f2fac46f4d0180c0d58e08e179e7c12 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:56:56 +0200 Subject: [PATCH 078/103] forgotten define portenta --- src/drivers/hardware_specific/portenta_h7_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index 74a66348..b314ee40 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if 1 //defined(TARGET_PORTENTA_H7) +#if defined(TARGET_PORTENTA_H7) #include "pwmout_api.h" #include "pinDefinitions.h" From 8a86629308e074958cc5678a3e1d213340caf4c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 14:07:20 +0200 Subject: [PATCH 079/103] stm32 bugfix --- src/drivers/hardware_specific/stm32_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 215ece87..5efddf41 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -72,11 +72,11 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; #if defined(TIM_OCIDLESTATE_SET) - sConfig.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; #endif #if defined(TIM_OCNIDLESTATE_RESET) - sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; #endif uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); From a55da2da8dd1cb1ab770fe515288dc85b90f2f7a Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 28 Apr 2021 16:46:37 +0200 Subject: [PATCH 080/103] Implement current sense for STM32 based B-G431B-ESC1 for this board, use InlineCurrentSense with: shunt_resistor = 0.003 gain = -64.0/7.0 --- .../hardware_specific/generic_mcu.cpp | 1 - .../hardware_specific/stm32_mcu.cpp | 2 +- .../hardware_specific/stm32g4_hal.cpp | 333 ++++++++++++++++++ .../hardware_specific/stm32g4_hal.h | 14 + .../hardware_specific/stm32g4_mcu.cpp | 122 +++++++ src/drivers/hardware_specific/stm32_mcu.cpp | 9 + 6 files changed, 479 insertions(+), 2 deletions(-) create mode 100644 src/current_sense/hardware_specific/stm32g4_hal.cpp create mode 100644 src/current_sense/hardware_specific/stm32g4_hal.h create mode 100644 src/current_sense/hardware_specific/stm32g4_mcu.cpp diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index ecc0b79b..50f383ad 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -26,7 +26,6 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c if( _isset(pinC) ) pinMode(pinC, INPUT); } - // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } __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 19833e11..797ac6a1 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) and !defined(STM32G4xx) #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f diff --git a/src/current_sense/hardware_specific/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32g4_hal.cpp new file mode 100644 index 00000000..f0204566 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_hal.cpp @@ -0,0 +1,333 @@ +#include "../hardware_api.h" +#if defined(STM32G4xx) + +#include "stm32g4xx_hal.h" +#include "stm32g4xx_ll_pwr.h" +#include "stm32g4xx_hal_adc.h" +#include "stm32g4_hal.h" +// From STM32 cube IDE +/** + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2020 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +void MX_GPIO_Init(void) +{ + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOF_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + __HAL_RCC_ADC12_CLK_ENABLE(); +} + +/** + * Enable DMA controller clock + */ +void MX_DMA_Init(void) +{ + /* DMA controller clock enable */ + __HAL_RCC_DMAMUX1_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); + /* DMA1_Channel2_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn); + + // Enable external clock for ADC + RCC_PeriphCLKInitTypeDef PeriphClkInit; + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12; + PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_PLL; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); +} + + +/** + * @brief ADC1 Initialization Function + * @param None + * @retval None + */ +void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) +{ + /* USER CODE BEGIN ADC1_Init 0 */ + + /* USER CODE END ADC1_Init 0 */ + + ADC_MultiModeTypeDef multimode = {0}; + ADC_ChannelConfTypeDef sConfig = {0}; + + /* USER CODE BEGIN ADC1_Init 1 */ + + /* USER CODE END ADC1_Init 1 */ + /** Common config + */ + hadc1->Instance = ADC1; + hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16; + hadc1->Init.Resolution = ADC_RESOLUTION_12B; + hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1->Init.GainCompensation = 0; + hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc1->Init.LowPowerAutoWait = DISABLE; + hadc1->Init.ContinuousConvMode = DISABLE; + hadc1->Init.NbrOfConversion = 2; + hadc1->Init.DiscontinuousConvMode = DISABLE; + hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc1->Init.DMAContinuousRequests = ENABLE; + hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc1) != HAL_OK) + { + Error_Handler(); + } + + /** Configure the ADC multi-mode + */ + multimode.Mode = ADC_MODE_INDEPENDENT; + if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_12; // ADC1_IN12 = PB1 = OP3_OUT + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; // ADC1_IN3 = PA2 = OP1_OUT + sConfig.Rank = ADC_REGULAR_RANK_2; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN ADC1_Init 2 */ + + /* USER CODE END ADC1_Init 2 */ + +} + +/** + * @brief ADC2 Initialization Function + * @param None + * @retval None + */ +void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) +{ + /* USER CODE BEGIN ADC2_Init 0 */ + + /* USER CODE END ADC2_Init 0 */ + + ADC_ChannelConfTypeDef sConfig = {0}; + + /* USER CODE BEGIN ADC2_Init 1 */ + + /* USER CODE END ADC2_Init 1 */ + /** Common config + */ + hadc2->Instance = ADC2; + hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16; + hadc2->Init.Resolution = ADC_RESOLUTION_12B; + hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2->Init.GainCompensation = 0; + hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc2->Init.LowPowerAutoWait = DISABLE; + hadc2->Init.ContinuousConvMode = DISABLE; + hadc2->Init.NbrOfConversion = 1; + hadc2->Init.DiscontinuousConvMode = DISABLE; + hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc2->Init.DMAContinuousRequests = ENABLE; + hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc2) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; // ADC2_IN3 = PA6 + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN ADC2_Init 2 */ + + /* USER CODE END ADC2_Init 2 */ + +} + +/** +* @brief OPAMP MSP Initialization +* This function configures the hardware resources used in this example +* @param hopamp-> OPAMP handle pointer +* @retval None +*/ +void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* hopamp) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(hopamp->Instance==OPAMP1) + { + /* USER CODE BEGIN OPAMP1_MspInit 0 */ + + /* USER CODE END OPAMP1_MspInit 0 */ + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**OPAMP1 GPIO Configuration + PA1 ------> OPAMP1_VINP + PA2 ------> OPAMP1_VOUT + PA3 ------> OPAMP1_VINM + */ + GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP1_MspInit 1 */ + + /* USER CODE END OPAMP1_MspInit 1 */ + } + else if(hopamp->Instance==OPAMP2) + { + /* USER CODE BEGIN OPAMP2_MspInit 0 */ + + /* USER CODE END OPAMP2_MspInit 0 */ + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**OPAMP2 GPIO Configuration + PA5 ------> OPAMP2_VINM + PA6 ------> OPAMP2_VOUT + PA7 ------> OPAMP2_VINP + */ + GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP2_MspInit 1 */ + + /* USER CODE END OPAMP2_MspInit 1 */ + } + else if(hopamp->Instance==OPAMP3) + { + /* USER CODE BEGIN OPAMP3_MspInit 0 */ + + /* USER CODE END OPAMP3_MspInit 0 */ + + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**OPAMP3 GPIO Configuration + PB0 ------> OPAMP3_VINP + PB1 ------> OPAMP3_VOUT + PB2 ------> OPAMP3_VINM + */ + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP3_MspInit 1 */ + + /* USER CODE END OPAMP3_MspInit 1 */ + } + +} + +/** +* @brief OPAMP MSP De-Initialization +* This function freeze the hardware resources used in this example +* @param hopamp-> OPAMP handle pointer +* @retval None +*/ +void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp) +{ + if(hopamp->Instance==OPAMP1) + { + /* USER CODE BEGIN OPAMP1_MspDeInit 0 */ + + /* USER CODE END OPAMP1_MspDeInit 0 */ + + /**OPAMP1 GPIO Configuration + PA1 ------> OPAMP1_VINP + PA2 ------> OPAMP1_VOUT + PA3 ------> OPAMP1_VINM + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3); + + /* USER CODE BEGIN OPAMP1_MspDeInit 1 */ + + /* USER CODE END OPAMP1_MspDeInit 1 */ + } + else if(hopamp->Instance==OPAMP2) + { + /* USER CODE BEGIN OPAMP2_MspDeInit 0 */ + + /* USER CODE END OPAMP2_MspDeInit 0 */ + + /**OPAMP2 GPIO Configuration + PA5 ------> OPAMP2_VINM + PA6 ------> OPAMP2_VOUT + PA7 ------> OPAMP2_VINP + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7); + + /* USER CODE BEGIN OPAMP2_MspDeInit 1 */ + + /* USER CODE END OPAMP2_MspDeInit 1 */ + } + else if(hopamp->Instance==OPAMP3) + { + /* USER CODE BEGIN OPAMP3_MspDeInit 0 */ + + /* USER CODE END OPAMP3_MspDeInit 0 */ + + /**OPAMP3 GPIO Configuration + PB0 ------> OPAMP3_VINP + PB1 ------> OPAMP3_VOUT + PB2 ------> OPAMP3_VINM + */ + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2); + + /* USER CODE BEGIN OPAMP3_MspDeInit 1 */ + + /* USER CODE END OPAMP3_MspDeInit 1 */ + } + +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32g4_hal.h new file mode 100644 index 00000000..13dd1de3 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_hal.h @@ -0,0 +1,14 @@ +#ifndef stm32g4_hal +#define stm32g4_hal + +#if defined(STM32G4xx) +void MX_GPIO_Init(void); +void MX_DMA_Init(void); +void MX_ADC1_Init(ADC_HandleTypeDef* hadc1); +void MX_ADC2_Init(ADC_HandleTypeDef* hadc2); +void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp); +void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp); +void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp); +#endif + +#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 new file mode 100644 index 00000000..f05f3514 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_mcu.cpp @@ -0,0 +1,122 @@ +#include "../hardware_api.h" +#include "stm32g4_hal.h" + +#if defined(STM32G4xx) +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 4096.0 +#define ADC_BUF_LEN_1 2 +#define ADC_BUF_LEN_2 1 + +static ADC_HandleTypeDef hadc1; +static ADC_HandleTypeDef hadc2; +static OPAMP_HandleTypeDef hopamp1; +static OPAMP_HandleTypeDef hopamp2; +static OPAMP_HandleTypeDef hopamp3; + +static DMA_HandleTypeDef hdma_adc1; +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){ + uint32_t raw_adc = 0; + if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer1[1]; + else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer2[0]; + else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer1[0]; + + return raw_adc * _ADC_CONV; +} + +void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ + // could this be replaced with LL_OPAMP calls?? + hopamp->Instance = OPAMPx_Def; + hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; + hopamp->Init.Mode = OPAMP_PGA_MODE; + hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; + hopamp->Init.InternalOutput = DISABLE; + hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; + hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; + hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; + hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; + if (HAL_OPAMP_Init(hopamp) != HAL_OK) + { + Error_Handler(); + } +} +void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ + // Configure the opamps + _configureOPAMP(OPAMPA, OPAMP1); + _configureOPAMP(OPAMPB, OPAMP2); + _configureOPAMP(OPAMPC, OPAMP3); +} + +void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) { + hdma_adc->Instance = channel; + hdma_adc->Init.Request = request; + hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE; + hdma_adc->Init.MemInc = DMA_MINC_ENABLE; + hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc->Init.Mode = DMA_CIRCULAR; + hdma_adc->Init.Priority = DMA_PRIORITY_LOW; + HAL_DMA_DeInit(hdma_adc); + if (HAL_DMA_Init(hdma_adc) != HAL_OK) + { + Error_Handler(); + } + __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); +} + +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + HAL_Init(); + MX_GPIO_Init(); + MX_DMA_Init(); + _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2); + MX_ADC1_Init(&hadc1); + MX_ADC2_Init(&hadc2); + + MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); + MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); + + if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) + { + Error_Handler(); + } + if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) + { + Error_Handler(); + } + + HAL_OPAMP_Start(&hopamp1); + HAL_OPAMP_Start(&hopamp2); + HAL_OPAMP_Start(&hopamp3); + + // Check if the ADC DMA is collecting any data. + // If this fails, it likely means timer1 has not started. Verify that your application starts + // 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(); + } +} + +extern "C" { +void DMA1_Channel1_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc1); +} + +void DMA1_Channel2_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc2); +} +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 5efddf41..540dd6c2 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -141,6 +141,7 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); 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); } @@ -159,8 +160,16 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + // Set Trigger out for DMA transfer + LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + HT->pause(); HT->refresh(); + + // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs. + HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + HT->getHandle()->Instance->CNT = TIM1->ARR; + HT->resume(); return HT; #else From c1ce9d7081edbbfc3631c5f8eecbb1271e699ef3 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 28 Apr 2021 17:46:32 +0200 Subject: [PATCH 081/103] add example for B-G431B-ESC1 current sense --- .../B_G431B_ESC1_position_control.ino | 117 ++++++++++++++++++ .../B_G431B_ESC1/build_opt.h | 1 + 2 files changed, 118 insertions(+) create mode 100644 examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino create mode 100644 examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino new file mode 100644 index 00000000..41d9148b --- /dev/null +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino @@ -0,0 +1,117 @@ +/** + * B-G431B-ESC1 position motion control example with encoder + * + */ +#include + +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver6PWM driver = BLDCDriver6PWM(PHASE_UH, PHASE_UL, PHASE_VH, PHASE_VL, PHASE_WH, PHASE_WL); +InlineCurrentSense currentSense = InlineCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT); + + +// encoder instance +Encoder encoder = Encoder(HALL2, HALL3, 2048, HALL1); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} +void doIndex(){encoder.handleIndex();} + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // current sensing + currentSense.init(); + motor.linkCurrentSense(¤tSense); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + // index search velocity [rad/s] + motor.velocity_index_search = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle P controller + motor.P_angle.P = 20; + // maximal velocity of the position control + motor.velocity_limit = 4; + + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target angle"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); + _delay(1000); +} + +// angle set point variable +float target_angle = 0; + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_angle); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h b/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h new file mode 100644 index 00000000..6f547ecd --- /dev/null +++ b/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h @@ -0,0 +1 @@ +-DHAL_OPAMP_MODULE_ENABLED \ No newline at end of file From 48192e01f1c4c1550e875b965cdd75f8a97ba0c4 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 07:46:38 +0200 Subject: [PATCH 082/103] issue #102 : update exxmple esp32 and added the sample code for second spi HSPI --- .github/workflows/ccpp.yml | 6 +-- .../esp32_position_control.ino | 2 +- .../esp32_spi_alt_example.ino | 41 +++++++++++++++++++ .../stm32_spi_alt_example.ino} | 0 4 files changed, 45 insertions(+), 4 deletions(-) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino rename examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/{magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino => magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino} (100%) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 07bb15b1..65e921fd 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -38,15 +38,15 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino + 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, magnetic_sensor_spi_alt_example.ino + 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 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: single_full_control_example.ino + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index d66b5e16..f025895b 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -8,7 +8,7 @@ // MOSI 9 // SCK 14 // magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 15); // I2C Magnetic sensor instance (AS5600 example) // make sure to use the pull-ups!! diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino new file mode 100644 index 00000000..395d3c31 --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino @@ -0,0 +1,41 @@ +#include + +// alternative pinout +#define HSPI_MISO 12 +#define HSPI_MOSI 13 +#define HSPI_SCLK 14 +#define HSPI_SS 15 + +// MagneticSensorSPI(int cs, float _cpr, int _angle_register) +// config - SPI config +// cs - SPI chip select pin +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, HSPI_SS); + +// for esp 32, it has 2 spi interfaces VSPI (default) and HPSI as the second one +// to enable it instatiate the object +SPIClass SPI_2(HSPI) + +void setup() { + // monitoring port + Serial.begin(115200); + + // start the newly defined spi communication + SPI_2.begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_SS); //SCLK, MISO, MOSI, SS + // initialise magnetic sensor hardware + sensor.init(&SPI_2); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino similarity index 100% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino From 6fc6d44db125c64daa96c273f8f2b4067e470e89 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 07:47:44 +0200 Subject: [PATCH 083/103] forgotten excludes --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 65e921fd..94184e71 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,7 +21,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From dfb1b3cd89bb421d3dcf1c218902e37381229cbc Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 07:51:57 +0200 Subject: [PATCH 084/103] issue #112 --- .../esp32_spi_alt_example/esp32_spi_alt_example.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino index 395d3c31..cbb72af4 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino @@ -13,7 +13,7 @@ MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, HSPI_SS); // for esp 32, it has 2 spi interfaces VSPI (default) and HPSI as the second one // to enable it instatiate the object -SPIClass SPI_2(HSPI) +SPIClass SPI_2(HSPI); void setup() { // monitoring port From 6053c1c35d8abf372c5d2290a2cd29aa523564a2 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 08:06:16 +0200 Subject: [PATCH 085/103] reqdme update after pr #73 --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index fe7ab263..568cb083 100644 --- a/README.md +++ b/README.md @@ -17,12 +17,13 @@ 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) ##### Release notes: SimpleFOClibrary v2.2 -> - Sensor floating point error bugfux #83 +> - Sensor floating point error bugfux #83 > - Support for portenta h7 board > - Support for arduino leonardo #108 > - Support for esp8266 > - Low side current sensing support for esp32 > - Restructured the generic code and simplified adding new mcus +> - Awesome :smiley: Low side current sening support for B_G431B_ESC1 by [@sDessens](https://github.com/sDessens): PR #73 ## Arduino *SimpleFOClibrary* v2.1 From dfe4222b3628d41982733b8afe7e1d4ab4c17f3a Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 08:11:37 +0200 Subject: [PATCH 086/103] current sense filtering #102 in monitor --- src/common/base_classes/FOCMotor.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 41c0dfd2..922076a6 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -108,15 +108,19 @@ void FOCMotor::monitor() { DQCurrent_s c{0,0}; if(current_sense){ if(torque_controller == TorqueControlType::foc_current) c = current; - else c = current_sense->getFOCCurrents(electrical_angle); + else{ + c = current_sense->getFOCCurrents(electrical_angle); + c.q = LPF_current_q(c.q); + c.d = LPF_current_d(c.d); + } } if(monitor_variables & _MON_CURR_Q) { - monitor_port->print(c.q*1000,2); // mAmps + monitor_port->print(c.q*1000, 2); // mAmps monitor_port->print("\t"); printed= true; } if(monitor_variables & _MON_CURR_D) { - monitor_port->print(c.d*1000,2); // mAmps + monitor_port->print(c.d*1000, 2); // mAmps monitor_port->print("\t"); printed= true; } From a64162cb2a0813915882bed6e4d13a26ec6b2197 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 09:02:43 +0200 Subject: [PATCH 087/103] forgotten exclude B_G431B --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 94184e71..6171ab77 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,7 +21,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1_position_control - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 461ed578f987eba2e0959558557d0be6ec14ecc2 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 15:52:22 +0200 Subject: [PATCH 088/103] shaft angle addded: forgotten the sensor direction --- .../{B_G431B_ESC1_position_control.ino => B_G431B_ESC1.ino} | 0 src/BLDCMotor.cpp | 3 +-- 2 files changed, 1 insertion(+), 2 deletions(-) rename examples/hardware_specific_examples/B_G431B_ESC1/{B_G431B_ESC1_position_control.ino => B_G431B_ESC1.ino} (100%) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino similarity index 100% rename from examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1_position_control.ino rename to examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index b0c84ba4..ff451e07 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -102,7 +102,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); - shaft_angle = sensor->getAngle(); + shaft_angle = shaftAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -270,7 +270,6 @@ void BLDCMotor::loopFOC() { // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() // which is in range 0-2PI electrical_angle = electricalAngle(); - switch (torque_controller) { case TorqueControlType::voltage: // no need to do anything really From f927db2f68c119214f8de23763897859038e6125 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 15:53:52 +0200 Subject: [PATCH 089/103] remove the initial glitch #111 and #110 --- src/common/base_classes/Sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index a5ad1b6d..23a51d9b 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; + if(Ts <= 0) Ts = 1e-3f; // velocity calculation float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; // save variables for future pass From e6836b360c403e1ac53cae2fe84a03d1ee2b7f36 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 15:54:43 +0200 Subject: [PATCH 090/103] antiwindup update Foxboro type #59 --- src/common/lowpass_filter.cpp | 9 ++++++--- src/common/pid.cpp | 6 ++++-- src/common/pid.h | 6 ++++-- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/common/lowpass_filter.cpp b/src/common/lowpass_filter.cpp index 4a643744..ffb15cfc 100644 --- a/src/common/lowpass_filter.cpp +++ b/src/common/lowpass_filter.cpp @@ -13,12 +13,15 @@ float LowPassFilter::operator() (float x) unsigned long timestamp = _micros(); float dt = (timestamp - timestamp_prev)*1e-6f; - if (dt < 0.0f || dt > 0.5f) - dt = 1e-3f; + if (dt < 0.0f ) dt = 1e-3f; + else if(dt > 0.3f) { + y_prev = x; + timestamp_prev = timestamp; + return x; + } float alpha = Tf/(Tf + dt); float y = alpha*y_prev + (1.0f - alpha)*x; - y_prev = y; timestamp_prev = timestamp; return y; diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 6472c0f4..cea8e199 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -28,9 +28,11 @@ float PIDController::operator() (float error){ float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) - float integral = integral_prev + I*Ts*0.5f*(error + error_prev); + // method uses the antiwindup Foxboro method : https://core.ac.uk/download/pdf/289952713.pdf + float integral = integral_prev + I*Ts*0.5f*(error + error_prev) + integral_antiwindup_prev*I; // antiwindup - limit the output - integral = _constrain(integral, -limit, limit); + float integral_constrained = _constrain(integral, -limit, limit); + integral_antiwindup_prev = integral - integral_constrained; // Discrete derivation // u_dk = D(ek - ek_1)/Ts float derivative = D*(error - error_prev)/Ts; diff --git a/src/common/pid.h b/src/common/pid.h index 39622ace..1a8ff434 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -30,11 +30,13 @@ class PIDController float output_ramp; //!< Maximum speed of change of the output value float limit; //!< Maximum output value -protected: + float output_prev; //!< last pid output value float integral_prev; //!< last integral component value + +protected: + float integral_antiwindup_prev; //!< last integral antiwindup component value float error_prev; //!< last tracking error value unsigned long timestamp_prev; //!< Last execution timestamp - float output_prev; //!< last pid output value }; #endif // PID_H \ No newline at end of file From 4fe57a8073210e8f992997da6a7c648f01f602f7 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 16:02:02 +0200 Subject: [PATCH 091/103] typo in antiwindup #59 --- src/common/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/pid.cpp b/src/common/pid.cpp index cea8e199..d24a1da6 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -29,7 +29,7 @@ float PIDController::operator() (float error){ // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) // method uses the antiwindup Foxboro method : https://core.ac.uk/download/pdf/289952713.pdf - float integral = integral_prev + I*Ts*0.5f*(error + error_prev) + integral_antiwindup_prev*I; + float integral = integral_prev + I*Ts*0.5f*(error + error_prev) - integral_antiwindup_prev*I; // antiwindup - limit the output float integral_constrained = _constrain(integral, -limit, limit); integral_antiwindup_prev = integral - integral_constrained; From 71652eb44e869ce51f692aa72bc65c673f3560b1 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 16:06:54 +0200 Subject: [PATCH 092/103] private vars --- src/common/pid.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/pid.h b/src/common/pid.h index 1a8ff434..8f1e812c 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -30,10 +30,10 @@ class PIDController float output_ramp; //!< Maximum speed of change of the output value float limit; //!< Maximum output value +protected: + float output_prev; //!< last pid output value float integral_prev; //!< last integral component value - -protected: float integral_antiwindup_prev; //!< last integral antiwindup component value float error_prev; //!< last tracking error value unsigned long timestamp_prev; //!< Last execution timestamp From ec2d96e00c226186ff20673ac431f2e667cc49e0 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 16 Sep 2021 16:10:47 +0200 Subject: [PATCH 093/103] unset variables --- src/common/pid.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/common/pid.cpp b/src/common/pid.cpp index d24a1da6..3fe1c323 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -9,6 +9,7 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , integral_prev(0.0f) , error_prev(0.0f) , output_prev(0.0f) + , integral_antiwindup_prev(0.0f) { timestamp_prev = _micros(); } From af78dac41b7728cadc49aa162efe5dda1609e715 Mon Sep 17 00:00:00 2001 From: SKURIC Antun Date: Mon, 20 Sep 2021 10:22:31 +0200 Subject: [PATCH 094/103] esp32 compile error --- src/common/pid.cpp | 2 +- src/common/pid.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 3fe1c323..f13d57ba 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -6,9 +6,9 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , D(D) , output_ramp(ramp) // output derivative limit [volts/second] , limit(limit) // output supply limit [volts] - , integral_prev(0.0f) , error_prev(0.0f) , output_prev(0.0f) + , integral_prev(0.0f) , integral_antiwindup_prev(0.0f) { timestamp_prev = _micros(); diff --git a/src/common/pid.h b/src/common/pid.h index 8f1e812c..6fe351f4 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -31,11 +31,10 @@ class PIDController float limit; //!< Maximum output value protected: - + float error_prev; //!< last tracking error value float output_prev; //!< last pid output value float integral_prev; //!< last integral component value float integral_antiwindup_prev; //!< last integral antiwindup component value - float error_prev; //!< last tracking error value unsigned long timestamp_prev; //!< Last execution timestamp }; From 24441b139a4338754470142dac8b570c7a850a06 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 29 Sep 2021 19:17:49 +0200 Subject: [PATCH 095/103] duplicated guards resolve #115 --- src/current_sense/hardware_api.h | 4 ++-- src/drivers/hardware_api.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index f715ef9e..17469bd3 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -1,5 +1,5 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#ifndef HARDWARE_UTILS_CURRENT_H +#define HARDWARE_UTILS_CURRENT_H #include "../common/foc_utils.h" #include "../common/time_utils.h" diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 01c03867..0b4f7c65 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -1,5 +1,5 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#ifndef HARDWARE_UTILS_DRIVER_H +#define HARDWARE_UTILS_DRIVER_H #include "../common/foc_utils.h" #include "../common/time_utils.h" From 657d339040a899a7369fb3f739b841c549ce7724 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 29 Sep 2021 20:47:04 +0200 Subject: [PATCH 096/103] esp32 fix reboot error --- src/common/foc_utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 75bce025..e0358750 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -25,7 +25,7 @@ #define _3PI_2 4.71238898038f #define _PI_6 0.52359877559f -#define NOT_SET -12345.0f +#define NOT_SET -12345.0 #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 From 3d142b7a5a73220d668b05ff98f051c1ce5b69ca Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 2 Oct 2021 20:50:09 +0200 Subject: [PATCH 097/103] remove the anti-windup --- src/common/pid.cpp | 6 ++---- src/common/pid.h | 1 - 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/common/pid.cpp b/src/common/pid.cpp index f13d57ba..51b6c56d 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -9,7 +9,6 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , error_prev(0.0f) , output_prev(0.0f) , integral_prev(0.0f) - , integral_antiwindup_prev(0.0f) { timestamp_prev = _micros(); } @@ -30,10 +29,9 @@ float PIDController::operator() (float error){ // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) // method uses the antiwindup Foxboro method : https://core.ac.uk/download/pdf/289952713.pdf - float integral = integral_prev + I*Ts*0.5f*(error + error_prev) - integral_antiwindup_prev*I; + float integral = integral_prev + I*Ts*0.5f*(error + error_prev); // antiwindup - limit the output - float integral_constrained = _constrain(integral, -limit, limit); - integral_antiwindup_prev = integral - integral_constrained; + integral = _constrain(integral, -limit, limit); // Discrete derivation // u_dk = D(ek - ek_1)/Ts float derivative = D*(error - error_prev)/Ts; diff --git a/src/common/pid.h b/src/common/pid.h index 6fe351f4..203d3c82 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -34,7 +34,6 @@ class PIDController float error_prev; //!< last tracking error value float output_prev; //!< last pid output value float integral_prev; //!< last integral component value - float integral_antiwindup_prev; //!< last integral antiwindup component value unsigned long timestamp_prev; //!< Last execution timestamp }; From fbd00f09a337d56eacefb646e9547b9f54ff1ec4 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 2 Oct 2021 20:57:37 +0200 Subject: [PATCH 098/103] readme update --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 568cb083..d36404aa 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,7 @@ Therefore this is an attempt to: > - Low side current sensing support for esp32 > - Restructured the generic code and simplified adding new mcus > - Awesome :smiley: Low side current sening support for B_G431B_ESC1 by [@sDessens](https://github.com/sDessens): PR #73 +> - **The release will be mande as soon as the docs are ready** ## Arduino *SimpleFOClibrary* v2.1 From 13a389d821f52dc914fb4f7b26845dcdd1224385 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 2 Oct 2021 23:17:31 +0200 Subject: [PATCH 099/103] fix merge errors --- .../hardware_specific/samd21_mcu.cpp | 42 ------------------- 1 file changed, 42 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index af2c0048..8bb6d38c 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -72,11 +72,7 @@ static void adcStopWithDMA(void); static void adcStartWithDMA(void); /** -<<<<<<< HEAD - * @brief ADC sync wait -======= * @brief ADC sync wait ->>>>>>> dev * @retval void */ static __inline__ void ADCsync() __attribute__((always_inline, unused)); @@ -86,15 +82,9 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -<<<<<<< HEAD -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() -{ - -======= SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() { ->>>>>>> dev return &instance; } @@ -146,11 +136,7 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { } void SAMDCurrentSenseADCDMA::initPins(){ -<<<<<<< HEAD - -======= ->>>>>>> dev pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -159,11 +145,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); -<<<<<<< HEAD - if( _isset(pinC) ) -======= if( _isset(pinC) ) ->>>>>>> dev { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); @@ -178,15 +160,6 @@ void SAMDCurrentSenseADCDMA::initPins(){ void SAMDCurrentSenseADCDMA::initADC(){ -<<<<<<< HEAD - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() - - ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC - ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297f V Supply VDDANA -======= analogRead(pinA); // do some pin init pinPeripheral() analogRead(pinB); // do some pin init pinPeripheral() analogRead(pinC); // do some pin init pinPeripheral() @@ -194,7 +167,6 @@ void SAMDCurrentSenseADCDMA::initADC(){ ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA ->>>>>>> dev 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; @@ -243,11 +215,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ */ ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); -<<<<<<< HEAD - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) -======= ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ->>>>>>> dev ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -278,15 +246,9 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); -<<<<<<< HEAD - - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) -======= DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) ->>>>>>> dev | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -327,11 +289,7 @@ void adcStartWithDMA(void){ ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; ADCsync(); -<<<<<<< HEAD - ADC->CTRLA.bit.ENABLE = 0x01; -======= ADC->CTRLA.bit.ENABLE = 0x01; ->>>>>>> dev ADCsync(); } From 3fff22aa1141c4e670863578f1da29e82c8588e5 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 3 Oct 2021 12:47:06 +0200 Subject: [PATCH 100/103] inline to low-side for BG431B ESC1 and pid removed antiwindup comment --- .../B_G431B_ESC1/B_G431B_ESC1.ino | 11 +++-------- src/common/pid.cpp | 1 - src/current_sense/LowsideCurrentSense.cpp | 16 +++++----------- src/current_sense/hardware_api.h | 3 --- .../hardware_specific/stm32g4_mcu.cpp | 8 ++++++++ 5 files changed, 16 insertions(+), 23 deletions(-) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 41d9148b..83451aa7 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -7,7 +7,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); BLDCDriver6PWM driver = BLDCDriver6PWM(PHASE_UH, PHASE_UL, PHASE_VH, PHASE_VL, PHASE_WH, PHASE_WL); -InlineCurrentSense currentSense = InlineCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT); +LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT); // encoder instance @@ -43,6 +43,8 @@ void setup() { // current sensing currentSense.init(); + // no need for aligning + currentSense.skip_align = true; motor.linkCurrentSense(¤tSense); // aligning voltage [V] @@ -97,15 +99,8 @@ float target_angle = 0; void loop() { // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code motor.move(target_angle); // function intended to be used with serial plotter to monitor motor variables diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 51b6c56d..aeeccf1a 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -28,7 +28,6 @@ float PIDController::operator() (float error){ float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) - // method uses the antiwindup Foxboro method : https://core.ac.uk/download/pdf/289952713.pdf float integral = integral_prev + I*Ts*0.5f*(error + error_prev); // antiwindup - limit the output integral = _constrain(integral, -limit, limit); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 2ff2ce8b..ca4f51b4 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -75,11 +75,7 @@ int LowsideCurrentSense::driverSync(BLDCDriver *driver){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ - //gain_a *= -1; - //gain_b *= -1; - //gain_c *= -1; - - /* + int exit_flag = 1; if(skip_align) return exit_flag; @@ -90,9 +86,9 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -165,7 +161,7 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 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.0; + c.c = (c.c+c1.c)/50.0f; } driver->setPwm(0, 0, 0); gain_c *= _sign(c.c); @@ -180,6 +176,4 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 4 - success but pins reconfigured and gains inverted return exit_flag; - */ - return 1; } diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 17469bd3..e1b50003 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -43,7 +43,4 @@ float _readADCVoltageLowSide(const int pinA); */ void _driverSyncLowSide(); - -void _startADC3PinConversionLowSide(); - #endif diff --git a/src/current_sense/hardware_specific/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32g4_mcu.cpp index f05f3514..5b5d00ee 100644 --- a/src/current_sense/hardware_specific/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32g4_mcu.cpp @@ -34,6 +34,10 @@ float _readADCVoltageInline(const int pin){ return raw_adc * _ADC_CONV; } +// do the same for low side sensing +float _readADCVoltageLowSide(const int pin){ + return _readADCVoltageInline(pin); +} void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ // could this be replaced with LL_OPAMP calls?? @@ -108,6 +112,10 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ Error_Handler(); } } +// do the same for low side +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _configureADCInline(pinA, pinB, pinC); +} extern "C" { void DMA1_Channel1_IRQHandler(void) { From 36226d0a51ebea9852128b5661c7b92bd58b7702 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 3 Oct 2021 16:32:02 +0200 Subject: [PATCH 101/103] typo in sensor.h --- src/common/base_classes/Sensor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 3a946bc7..efab1be1 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -28,8 +28,8 @@ enum Pullup{ * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the * most basic common features so that a FOC driver can obtain the data it needs for operation. * - * To implement your own sensors, create a sub-class of this class, and implement the getAngle() - * method. getAngle() returns a float value, in radians, representing the current shaft angle in the + * To implement your own sensors, create a sub-class of this class, and implement the getSensorAngle() + * method. getSensorAngle() returns a float value, in radians, representing the current shaft angle in the * range 0 to 2*PI (one full turn). * * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally, From 66d3c3345c591481a8ea9bd566a67982048c922c Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 3 Oct 2021 17:39:50 +0200 Subject: [PATCH 102/103] fuller readme --- README.md | 35 ++++++++++++++++++++++++++--------- 1 file changed, 26 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index d36404aa..bf5072c8 100644 --- a/README.md +++ b/README.md @@ -16,15 +16,32 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes: SimpleFOClibrary v2.2 -> - Sensor floating point error bugfux #83 -> - Support for portenta h7 board -> - Support for arduino leonardo #108 -> - Support for esp8266 -> - Low side current sensing support for esp32 -> - Restructured the generic code and simplified adding new mcus -> - Awesome :smiley: Low side current sening support for B_G431B_ESC1 by [@sDessens](https://github.com/sDessens): PR #73 -> - **The release will be mande as soon as the docs are ready** + +
+

NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

+
    +
  • 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 +
      +
    • Support for arduino leonardo #108 - docs
      • +
    • Initial support for portenta h7 board in collaboration with Arduino - docs
      • +
    • Initial support for esp8266 - docs
      • +
    +
    • +
  • 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
      • +
    +
    • +
+
## Arduino *SimpleFOClibrary* v2.1 From ed900161371baeba3cf9e3186f649fe50229f2f2 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 3 Oct 2021 18:02:18 +0200 Subject: [PATCH 103/103] fix typo in css --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index bf5072c8..c0e019d6 100644 --- a/README.md +++ b/README.md @@ -26,11 +26,11 @@ Therefore this is an attempt to:
  • 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 +
  • New MCU support - docs
      -
    • Support for arduino leonardo #108 - docs
      • -
    • Initial support for portenta h7 board in collaboration with Arduino - docs
      • -
    • Initial support for esp8266 - 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