From 678c6f2bd0e33309476991539fbc0e080a680317 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 6 Mar 2021 00:46:29 +0100 Subject: [PATCH 01/45] working on SAMD51 --- src/drivers/hardware_specific/generic_mcu.cpp | 2 + src/drivers/hardware_specific/samd21_mcu.cpp | 21 ++- .../samd21_wo_associations.h | 7 +- .../samd51_wo_associations.h | 124 ++++++++++++++++++ src/drivers/hardware_specific/samd_debug.h | 24 +++- 5 files changed, 163 insertions(+), 15 deletions(-) create mode 100644 src/drivers/hardware_specific/samd51_wo_associations.h diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 73aa3c9f..170b9dae 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -14,6 +14,8 @@ #elif defined(_SAMD21_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD51_) // samd21 for the moment, samd51 in progress... + #else // function setting the high pwm frequency to the supplied pins diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cdbd82f8..bb5822cb 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -1,30 +1,29 @@ -#if defined(ARDUINO_ARCH_SAMD) -//#if defined(_SAMD21_) - -#include "../hardware_api.h" -#include "wiring_private.h" - -#include "./samd21_wo_associations.h" +#define SIMPLEFOC_SAMD_DEBUG -#define SIMPLEFOC_SAMD_DEBUG +#include "../hardware_api.h" +#include "wiring_private.h" +#if defined(SAMD_SERIES) -#ifndef SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS -#define SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS false +#if defined(_SAMD21_) +#include "./samd21_wo_associations.h" +#elif defined(_SAMD51_) +#include "./samd21_wo_associations.h" #endif + #ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION #define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 #define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 #endif #ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS -#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 12 +#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 #endif diff --git a/src/drivers/hardware_specific/samd21_wo_associations.h b/src/drivers/hardware_specific/samd21_wo_associations.h index 3ab087e0..3e1ecd5e 100644 --- a/src/drivers/hardware_specific/samd21_wo_associations.h +++ b/src/drivers/hardware_specific/samd21_wo_associations.h @@ -1,4 +1,9 @@ +#include "variant.h" + +#ifdef _SAMD21_ + + struct wo_association { EPortType port; @@ -11,8 +16,6 @@ struct wo_association { -#ifdef _SAMD21_ - #ifndef TCC3_CH0 diff --git a/src/drivers/hardware_specific/samd51_wo_associations.h b/src/drivers/hardware_specific/samd51_wo_associations.h new file mode 100644 index 00000000..b59f13bb --- /dev/null +++ b/src/drivers/hardware_specific/samd51_wo_associations.h @@ -0,0 +1,124 @@ + + + +// TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation +// 0 6 8 24-bit Yes Yes Yes Yes Yes Yes +// 1 4 8 24-bit Yes Yes Yes Yes Yes Yes +// 2 3 3 16-bit Yes - Yes - - - +// 3 2 2 16-bit Yes - - - - - +// 4 2 2 16-bit Yes - - - - - + + +#include "variant.h" + + +#ifdef _SAMD51_ + + + +struct wo_association { + EPortType port; + uint32_t pin; + ETCChannel tccE; + uint8_t woE; + ETCChannel tccF; + uint8_t woF; + ETCChannel tccG; + uint8_t woG; +}; + + +struct wo_association WO_associations[] = { + + { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTA, 4, TC0_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTA, 5, TC0_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTA, 6, TC1_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTA, 7, TC1_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTC, 4, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, + // PC05, PC06, PC07 -> no timers + { PORTA, 8, TC0_CH0, 0, TCC0_CH0, 0, TCC1_CH0, 4}, + { 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, 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}, + { PORTB, 15, TC5_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 3}, + { 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}, //? + { 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}, + { 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}, + { 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}, + { 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, 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 + { PORTB, 21, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTA, 20, TC7_CH0, 0, TCC1_CH0, 4, TCC0_CH0, 0}, + { PORTA, 21, TC7_CH1, 1, TCC1_CH1, 5, TCC0_CH1, 1}, + { PORTA, 22, TC4_CH0, 0, TCC1_CH2, 6, TCC0_CH2, 2}, + { PORTA, 23, TC4_CH1, 1, TCC1_CH3, 7, TCC0_CH3, 3}, + { PORTA, 24, TC5_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, // PDEC0 + { PORTA, 25, TC5_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 22, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 23, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC0 + { PORTB, 24, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 25, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, + { PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0}, + { PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0}, + // 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}, + // 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}, + { PORTB, 2, TC6_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, + +}; +#define NUM_WO_ASSOCIATIONS 72 + +wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; + + +struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { + for (int i=0;iCTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC; +#if defined(_SAMD21_) +#include "./samd21_wo_associations.h" +#elif defined(_SAMD51_) +#include "./samd51_wo_associations.h" +#endif + //while (TCC0->SYNCBUSY.bit.CTRLB); // or while (TCC0->SYNCBUSY.reg); //int count = TCC0->COUNT.reg; @@ -66,6 +70,22 @@ void printAllPinInfos() { else Serial.println(" None "); +#ifdef _SAMD51_ + Serial.print(" G="); + if (association.tccG>=0) { + int tcn = GetTCNumber(association.tccG); + Serial.print(" TCC"); + Serial.print(tcn); + Serial.print("-"); + Serial.print(GetTCChannelNumber(association.tccG)); + Serial.print("["); + Serial.print(GetTCChannelNumber(association.woG)); + Serial.println("]"); + } + else + Serial.println(" None "); +#endif + } Serial.println(); From 0ba325c25849aa847647fef323e26187fbbe4287 Mon Sep 17 00:00:00 2001 From: Jordan Cormack <37104498+jordancormack@users.noreply.github.com> Date: Thu, 18 Mar 2021 22:17:38 +0000 Subject: [PATCH 02/45] Adding MagneticSensorPWM This implementation has been tested with the AS5048a PWM output. It follows an almost identical format to the existing MagneticSensorAnalog implementation. --- src/SimpleFOC.h | 1 + src/sensors/MagneticSensorPWM.cpp | 66 +++++++++++++++++++++++++++++++ src/sensors/MagneticSensorPWM.h | 57 ++++++++++++++++++++++++++ 3 files changed, 124 insertions(+) create mode 100644 src/sensors/MagneticSensorPWM.cpp create mode 100644 src/sensors/MagneticSensorPWM.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 6b7afbb2..eee0297b 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -102,6 +102,7 @@ void loop() { #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" #include "sensors/MagneticSensorAnalog.h" +#include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp new file mode 100644 index 00000000..22cfeeaf --- /dev/null +++ b/src/sensors/MagneticSensorPWM.cpp @@ -0,0 +1,66 @@ +#include "MagneticSensorPWM.h" + +/** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max) + * @param _pinPWM the pin that is reading the pwm from magnetic sensor + * @param _min_raw_count the smallest expected reading + * @param _max_raw_count the largest expected reading + */ +MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _max_raw_count){ + + pinPWM = _pinPWM; + + cpr = _max_raw_count - _min_raw_count; + min_raw_count = _min_raw_count; + max_raw_count = _max_raw_count; + + pinMode(pinPWM, INPUT); +} + + +void MagneticSensorPWM::init(){ + + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + raw_count_prev = getRawCount(); +} + +// 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; + + 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; +} + +// get velocity (rad/s) +float MagneticSensorPWM::getVelocity(){ + return velocity; +} + +// read the raw counter of the magnetic sensor +int MagneticSensorPWM::getRawCount(){ + return pulseIn(pinPWM,HIGH); +} \ No newline at end of file diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h new file mode 100644 index 00000000..4477340e --- /dev/null +++ b/src/sensors/MagneticSensorPWM.h @@ -0,0 +1,57 @@ +#ifndef MAGNETICSENSORPWM_LIB_H +#define MAGNETICSENSORPWM_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +// This sensor has been tested with AS5048a running in PWM mode. + +class MagneticSensorPWM: public Sensor{ + public: + /** + * MagneticSensorPWM class constructor + * @param _pinPWM the pin to read the PWM sensor input signal + */ + MagneticSensorPWM(uint8_t _pinPWM,int _min = 0, int _max = 0); + + + // initialize the sensor hardware + void init(); + + int pinPWM; + + // get current angle (rad) + float getAngle() override; + // get current angular velocity (rad/s) + float getVelocity() override; + + + private: + // raw count (typically in range of 0-1023) + int raw_count; + int min_raw_count; + int max_raw_count; + int cpr; + int read(); + + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //! Date: Thu, 18 Mar 2021 22:20:30 +0000 Subject: [PATCH 03/45] Added new line at end of files --- src/sensors/MagneticSensorPWM.cpp | 2 +- src/sensors/MagneticSensorPWM.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 22cfeeaf..7ae989be 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -63,4 +63,4 @@ float MagneticSensorPWM::getVelocity(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ return pulseIn(pinPWM,HIGH); -} \ No newline at end of file +} diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 4477340e..2957bcff 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -54,4 +54,4 @@ class MagneticSensorPWM: public Sensor{ }; -#endif \ No newline at end of file +#endif From 19e220dac5704efd4f353e6fe672203a51ea4f02 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 3 Apr 2021 23:56:13 +0200 Subject: [PATCH 04/45] SAMD refactoring - in progress --- src/drivers/hardware_specific/samd21_mcu.cpp | 905 +++--------------- .../samd21_wo_associations.h | 134 --- src/drivers/hardware_specific/samd51_mcu.cpp | 298 ++++++ .../samd51_wo_associations.h | 124 --- src/drivers/hardware_specific/samd_debug.h | 127 --- src/drivers/hardware_specific/samd_mcu.cpp | 862 +++++++++++++++++ src/drivers/hardware_specific/samd_mcu.h | 103 ++ 7 files changed, 1401 insertions(+), 1152 deletions(-) delete mode 100644 src/drivers/hardware_specific/samd21_wo_associations.h create mode 100644 src/drivers/hardware_specific/samd51_mcu.cpp delete mode 100644 src/drivers/hardware_specific/samd51_wo_associations.h delete mode 100644 src/drivers/hardware_specific/samd_debug.h create mode 100644 src/drivers/hardware_specific/samd_mcu.cpp create mode 100644 src/drivers/hardware_specific/samd_mcu.h diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index bb5822cb..0c56eef4 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -1,68 +1,142 @@ -#define SIMPLEFOC_SAMD_DEBUG +#include "./samd_mcu.h" +#ifdef _SAMD21_ -#include "../hardware_api.h" -#include "wiring_private.h" -#if defined(SAMD_SERIES) -#if defined(_SAMD21_) -#include "./samd21_wo_associations.h" -#elif defined(_SAMD51_) -#include "./samd21_wo_associations.h" +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER #endif - - -#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION -#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 -#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 +#ifndef TCC3_CH1 +#define TCC3_CH1 NOT_ON_TIMER #endif - -#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS -#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 +#ifndef TCC3_CH2 +#define TCC3_CH2 NOT_ON_TIMER +#endif +#ifndef TCC3_CH3 +#define TCC3_CH3 NOT_ON_TIMER +#endif +#ifndef TCC3_CH4 +#define TCC3_CH4 NOT_ON_TIMER +#endif +#ifndef TCC3_CH5 +#define TCC3_CH5 NOT_ON_TIMER +#endif +#ifndef TCC3_CH6 +#define TCC3_CH6 NOT_ON_TIMER +#endif +#ifndef TCC3_CH7 +#define TCC3_CH7 NOT_ON_TIMER +#endif +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#endif +#ifndef TC6_CH1 +#define TC6_CH1 NOT_ON_TIMER +#endif +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#endif +#ifndef TC7_CH1 +#define TC7_CH1 NOT_ON_TIMER #endif -// Wait for synchronization of registers between the clock domains -static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); -static void syncTCC(Tcc* TCCx) { - while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); -} +#define NUM_WO_ASSOCIATIONS 48 + +/* + * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices + * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway? + * + * Note: only the pins which have timers associated are listed in this table. + * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table. + * + * See Microchip Technology datasheet DS40001882F-page 30 + */ +struct wo_association WO_associations[] = { + + { PORTA, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, + { PORTA, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, + { PORTA, 2, NOT_ON_TIMER, 0, TCC3_CH0, 0}, + { PORTA, 3, NOT_ON_TIMER, 0, TCC3_CH1, 1}, + // PB04, PB05, PB06, PB07 - no timers + { PORTB, 8, TC4_CH0, 0, TCC3_CH6, 6}, + { PORTB, 9, TC4_CH1, 1, TCC3_CH7, 7}, + { PORTA, 4, TCC0_CH0, 0, TCC3_CH2, 2}, + { PORTA, 5, TCC0_CH1, 1, TCC3_CH3, 3}, + { PORTA, 6, TCC1_CH0, 0, TCC3_CH4, 4}, + { PORTA, 7, TCC1_CH1, 1, TCC3_CH5, 5}, + { PORTA, 8, TCC0_CH0, 0, TCC1_CH2, 2}, + { PORTA, 9, TCC0_CH1, 1, TCC1_CH3, 3}, + { PORTA, 10, TCC1_CH0, 0, TCC0_CH2, 2}, + { PORTA, 11, TCC1_CH1, 1, TCC0_CH3, 3}, + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4}, + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5}, + { PORTB, 12, TC4_CH0, 0, TCC0_CH6, 6}, + { PORTB, 13, TC4_CH1, 1, TCC0_CH7, 7}, + { PORTB, 14, TC5_CH0, 0, NOT_ON_TIMER, 0}, + { PORTB, 15, TC5_CH1, 1, NOT_ON_TIMER, 0}, + { PORTA, 12, TCC2_CH0, 0, TCC0_CH6, 6}, + { PORTA, 13, TCC2_CH1, 1, TCC0_CH7, 7}, + { PORTA, 14, TC3_CH0, 0, TCC0_CH4, 4}, + { PORTA, 15, TC3_CH1, 1, TCC0_CH5, 5}, + { PORTA, 16, TCC2_CH0, 0, TCC0_CH6, 6}, + { PORTA, 17, TCC2_CH1, 1, TCC0_CH7, 7}, + { PORTA, 18, TC3_CH0, 0, TCC0_CH2, 2}, + { PORTA, 19, TC3_CH1, 1, TCC0_CH3, 3}, + { PORTB, 16, TC6_CH0, 0, TCC0_CH4, 4}, + { PORTB, 17, TC6_CH1, 1, TCC0_CH5, 5}, + { PORTA, 20, TC7_CH0, 0, TCC0_CH6, 6}, + { PORTA, 21, TC7_CH1, 1, TCC0_CH7, 7}, + { PORTA, 22, TC4_CH0, 0, TCC0_CH4, 4}, + { PORTA, 23, TC4_CH1, 1, TCC0_CH5, 5}, + { PORTA, 24, TC5_CH0, 0, TCC1_CH2, 2}, + { PORTA, 25, TC5_CH1, 1, TCC1_CH3, 3}, + { PORTB, 22, TC7_CH0, 0, TCC3_CH0, 0}, + { PORTB, 23, TC7_CH1, 1, TCC3_CH1, 1}, + { PORTA, 27, NOT_ON_TIMER, 0, TCC3_CH6, 6}, + { PORTA, 28, NOT_ON_TIMER, 0, TCC3_CH7, 7}, + { PORTA, 30, TCC1_CH0, 0, TCC3_CH4, 4}, + { PORTA, 31, TCC1_CH1, 1, TCC3_CH5, 5}, + { PORTB, 30, TCC0_CH0, 0, TCC1_CH2, 2}, + { PORTB, 31, TCC0_CH1, 1, TCC1_CH3, 3}, + { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0}, + { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0}, + { PORTB, 2, TC6_CH0, 0, TCC3_CH2, 2}, + { PORTB, 3, TC6_CH1, 1, TCC3_CH3, 3} +}; +wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; -struct tccConfiguration { - uint8_t pin; - uint8_t alternate; // 1=true, 0=false - uint8_t wo; - union tccChanInfo { - struct { - int8_t chan; - int8_t tccn; - }; - uint16_t chaninfo; - } tcc; +struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { + for (int i=0;i>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; +} + + + + + +void syncTCC(Tcc* TCCx) { + while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); // Wait for synchronization of registers between the clock domains +} -/** - * Global state - */ -tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; -uint8_t numTccPinConfigurations = 0; -bool SAMDClockConfigured = false; -bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; /** * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that @@ -70,6 +144,10 @@ bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; * clocks. */ void configureSAMDClock() { + + // TODO investigate using the FDPLL96M clock to get 96MHz timer clocks... this + // would enable 48KHz PWM clocks, and setting the frequency between 24Khz with resolution 2000, to 48KHz with resolution 1000 + if (!SAMDClockConfigured) { SAMDClockConfigured = true; // mark clock as configured for (int i=0;i>1) { - case 0: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1);//GCLK_CLKCTRL_ID_TCC0_TCC1; - break; - case 1: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3);//GCLK_CLKCTRL_ID_TCC2_TC3; - break; - case 2: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5);//GCLK_CLKCTRL_ID_TC4_TC5; - break; - case 3: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); - break; - default: - return; + case 0: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1); break; //GCLK_CLKCTRL_ID_TCC0_TCC1; + case 1: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3); break; //GCLK_CLKCTRL_ID_TCC2_TC3; + case 2: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5); break; //GCLK_CLKCTRL_ID_TC4_TC5; + case 3: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); break; + default: return; } // Feed GCLK4 to TCC REG_GCLK_CLKCTRL = (uint16_t) GCLK_CLKCTRL_CLKEN | // Enable GCLK4 GCLK_CLKCTRL_GEN_GCLK4 | // Select GCLK4 GCLK_CLKCTRL_ID_ofthistcc; // Feed GCLK4 to tcc - while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization tccConfigured[tccConfig.tcc.tccn] = true; if (tccConfig.tcc.tccn>=TCC_INST_NUM) { Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); - // disable tc->COUNT8.CTRLA.bit.ENABLE = 0; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); @@ -148,7 +216,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f // enable 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); @@ -183,10 +250,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); } - // enable double buffering - //tcc->CTRLBCLR.bit.LUPD = 1; - //while ( tcc->SYNCBUSY.bit.CTRLB == 1 ); - // Enable TC tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync @@ -242,254 +305,26 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f -/** - * Attach the TCC to the pin - */ -bool attachTCC(tccConfiguration& tccConfig) { - if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS) - return false; - pinMode(tccConfig.pin, OUTPUT); - pinPeripheral(tccConfig.pin, (tccConfig.alternate==1)?EPioType::PIO_TIMER_ALT:EPioType::PIO_TIMER); - tccPinConfigurations[numTccPinConfigurations++] = tccConfig; - return true; -} - - - - - - -/** - * Check if the configuration is in use already. - */ -bool inUse(tccConfiguration& tccConfig) { - for (int i=0;i=TCC_INST_NUM) - return false; - - if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan) - return false; - if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan) - return false; - if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan) - return false; - if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan) - return false; - - if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan) - return false; - if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo) - return false; - - return true; -} - - - - -bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) { - for (int i=0;i=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM - || pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM) - return false; - - // check we're not in use - if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl)) - return false; - - // check pins are all different tccs/channels - if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - - // check H/L pins are on same timer - if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn) - return false; - - // check H/L pins aren't on both the same timer and wo - if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo) - return false; - - return true; -} - - - - - -int checkHardware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - for (int i=0;i<64;i++) { - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (i>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (i>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (i>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (i>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (i>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (i>>5&0x01)==0x1); - if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) - return i; - } - return -1; -} - - - - -int checkSoftware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - for (int i=0;i<64;i++) { - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (i>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (i>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (i>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (i>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (i>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (i>>5&0x01)==0x1); - if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) - return i; - } - return -1; -} - - - -int scorePermutation(tccConfiguration pins[], uint8_t num) { - uint32_t usedtccs = 0; - for (int i=0;i>1; - } - for (int i=0;i>1; - } - return score; -} - - - - - -int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) { - tccConfiguration tccConfs[num]; - int best = -1; - int bestscore = 1000000; - for (int i=0;i<(0x1<>j)&0x01)==0x1); - if (checkFunc(tccConfs, num)) { - int score = scorePermutation(tccConfs, num); - if (scoreCC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); - while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + // set via CC + //tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + //uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); + //while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB -// tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); -// tcc->STATUS.vec.CCBV = tcc->STATUS.vec.CCBV | (1<SYNCBUSY.reg & chanbit) > 0 ); + tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); + tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); + tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); + while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { Tc* tc = (Tc*)GetTC(chaninfo); - //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc);; + tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } } @@ -497,468 +332,4 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { - - - -/** - * Configuring PWM frequency, resolution and alignment - * - Stepper driver - 2PWM setting - * - hardware specific - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pinA pinA bldc driver - * @param pinB pinB bldc driver - */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[2] = { pinA, pinB }; - int compatibility = checkPermutations(2, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pinB, (compatibility>>1&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); -#endif - - // attach pins to timer peripherals - 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..."); -#endif - - // set up clock - Note: 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... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return; // Someone with a stepper-setup who can test it? -} - - - - - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - BLDC driver - 3PWM setting - * - hardware specific - * - * SAMD21 will support up to 2 BLDC motors in 3-PWM: - * one on TCC0 using 3 of the channels 0,1,2 or 3 - * one on TCC3 using 3 of the channels 0,1,2 or 3 - * i.e. 8 different pins can be used, but only 4 different signals (WO[x]) on those 8 pins - * WO[0] and WO[4] are the same - * WO[1] and WO[5] are the same - * WO[2] and WO[6] are the same - * WO[3] and WO[7] are the same - * - * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x] - * signal is on which pin of the Nano. You can drive one motor on TCC0. For other boards, consult their documentation. - * - * Note: - * That's if we want to keep the signals strictly in sync. - * - * If we can accept out-of-sync PWMs on the different phases, we could drive up to 4 BLDCs in 3-PWM mode, - * using all the TCC channels. (TCC0 & TCC3 - 4 channels each, TCC1 & TCC2 - 2 channels each) - * - * All channels will use the same resolution, prescaler and clock, but they will have different start-times leading - * to misaligned signals. - * - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pinA pinA bldc driver - * @param pinB pinB bldc driver - * @param pinC pinC bldc driver - */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[3] = { pinA, pinB, pinC }; - int compatibility = checkPermutations(3, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pinB, (compatibility>>1&0x01)==0x1), - getTCCChannelNr(pinC, (compatibility>>2&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); - printTCCConfiguration(tccConfs[2]); -#endif - - // attach pins to timer peripherals - attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... - attachTCC(tccConfs[1]); - attachTCC(tccConfs[2]); -#ifdef 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? - // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); - configureTCC(tccConfs[2], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - -} - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - Stepper driver - 4PWM setting - * - hardware specific - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pin1A pin1A stepper driver - * @param pin1B pin1B stepper driver - * @param pin2A pin2A stepper driver - * @param pin2B pin2B stepper driver - */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[4] = { pin1A, pin1B, pin2A, pin2B }; - int compatibility = checkPermutations(4, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pin1B, (compatibility>>1&0x01)==0x1), - getTCCChannelNr(pin2A, (compatibility>>2&0x01)==0x1), - getTCCChannelNr(pin2B, (compatibility>>3&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); - printTCCConfiguration(tccConfs[2]); - printTCCConfiguration(tccConfs[3]); -#endif - - // attach pins to timer peripherals - attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... - attachTCC(tccConfs[1]); - attachTCC(tccConfs[2]); - attachTCC(tccConfs[3]); -#ifdef 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? - // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); - configureTCC(tccConfs[2], pwm_frequency); - configureTCC(tccConfs[3], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return; // Someone with a stepper-setup who can test it? -} - - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - BLDC driver - 6PWM setting - * - hardware specific - * - * SAMD21 will support up to 2 BLDC motors in 6-PWM: - * one on TCC0 using 3 of the channels 0,1,2 or 3 - * one on TCC3 using 3 of the channels 0,1,2 or 3 - * i.e. 6 out of 8 pins must be used, in the following high/low side pairs: - * WO[0] & WO[4] (high side & low side) - * WO[1] & WO[5] - * WO[2] & WO[6] - * WO[3] & WO[7] - * - * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x] - * signal is on which pin of the Nano. You can drive 1 BLDC on TCC0. For other boards, consult their documentation. - * - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable - * @param pinA_h pinA high-side bldc driver - * @param pinA_l pinA low-side bldc driver - * @param pinB_h pinA high-side bldc driver - * @param pinB_l pinA low-side bldc driver - * @param pinC_h pinA high-side bldc driver - * @param pinC_l pinA low-side bldc driver - * - * @return 0 if config good, -1 if failed - */ -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - // we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int compatibility = checkHardware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - if (compatibility<0) { - compatibility = checkSoftware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return -1; - } - } - - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (compatibility>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (compatibility>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (compatibility>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (compatibility>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (compatibility>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (compatibility>>5&0x01)==0x1); - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); - printTCCConfiguration(pinAh); - printTCCConfiguration(pinAl); - printTCCConfiguration(pinBh); - printTCCConfiguration(pinBl); - printTCCConfiguration(pinCh); - printTCCConfiguration(pinCl); -#endif - - // attach pins to timer peripherals - bool allAttached = true; - allAttached |= attachTCC(pinAh); // in theory this can fail, but there is no way to signal it... - allAttached |= attachTCC(pinAl); - allAttached |= attachTCC(pinBh); - allAttached |= attachTCC(pinBl); - allAttached |= attachTCC(pinCh); - allAttached |= attachTCC(pinCl); - if (!allAttached) - return -1; -#ifdef 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 - configureSAMDClock(); - - // configure the TCC(s) - configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1); - if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo)) - configureTCC(pinAl, pwm_frequency, true, -1.0); - configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1); - if ((pinBh.tcc.chaninfo!=pinBl.tcc.chaninfo)) - configureTCC(pinBl, pwm_frequency, true, -1.0); - configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1); - if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) - configureTCC(pinCl, pwm_frequency, true, -1.0); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return 0; -} - - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - Stepper driver - 2PWM setting - * - hardware specific - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); - return; -} - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - BLDC driver - 3PWM setting - * - hardware specific - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param dc_c duty cycle phase C [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - * @param pinC phase C hardware pin number - */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); - tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); - return; -} - - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - Stepper driver - 4PWM setting - * - hardware specific - * - * @param dc_1a duty cycle phase 1A [0, 1] - * @param dc_1b duty cycle phase 1B [0, 1] - * @param dc_2a duty cycle phase 2A [0, 1] - * @param dc_2b duty cycle phase 2B [0, 1] - * @param pin1A phase 1A hardware pin number - * @param pin1B phase 1B hardware pin number - * @param pin2A phase 2A hardware pin number - * @param pin2B phase 2B hardware pin number - */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - tccConfiguration* tccI = getTccPinConfiguration(pin1A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); - tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); - tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); - tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); - return; -} - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - BLDC driver - 6PWM setting - * - hardware specific - * - * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored - * the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side - * duty cycle. - * No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method... - * so use appropriately. - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param dc_c duty cycle phase C [0, 1] - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - * @param pinA_h phase A high-side hardware pin number - * @param pinA_l phase A low-side hardware pin number - * @param pinB_h phase B high-side hardware pin number - * @param pinB_l phase B low-side hardware pin number - * @param pinC_h phase C high-side hardware pin number - * @param pinC_l phase C low-side hardware pin number - * - */ -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){ - tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); - tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); - 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 - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly - - tcc1 = getTccPinConfiguration(pinB_h); - tcc2 = getTccPinConfiguration(pinB_l); - if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - - tcc1 = getTccPinConfiguration(pinC_h); - tcc2 = getTccPinConfiguration(pinC_l); - if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - return; -} - - - - - - - #endif diff --git a/src/drivers/hardware_specific/samd21_wo_associations.h b/src/drivers/hardware_specific/samd21_wo_associations.h deleted file mode 100644 index 3e1ecd5e..00000000 --- a/src/drivers/hardware_specific/samd21_wo_associations.h +++ /dev/null @@ -1,134 +0,0 @@ - -#include "variant.h" - -#ifdef _SAMD21_ - - - -struct wo_association { - EPortType port; - uint32_t pin; - ETCChannel tccE; - uint8_t woE; - ETCChannel tccF; - uint8_t woF; -}; - - - - - -#ifndef TCC3_CH0 -#define TCC3_CH0 NOT_ON_TIMER -#endif -#ifndef TCC3_CH1 -#define TCC3_CH1 NOT_ON_TIMER -#endif -#ifndef TCC3_CH2 -#define TCC3_CH2 NOT_ON_TIMER -#endif -#ifndef TCC3_CH3 -#define TCC3_CH3 NOT_ON_TIMER -#endif -#ifndef TCC3_CH4 -#define TCC3_CH4 NOT_ON_TIMER -#endif -#ifndef TCC3_CH5 -#define TCC3_CH5 NOT_ON_TIMER -#endif -#ifndef TCC3_CH6 -#define TCC3_CH6 NOT_ON_TIMER -#endif -#ifndef TCC3_CH7 -#define TCC3_CH7 NOT_ON_TIMER -#endif -#ifndef TC6_CH0 -#define TC6_CH0 NOT_ON_TIMER -#endif -#ifndef TC6_CH1 -#define TC6_CH1 NOT_ON_TIMER -#endif -#ifndef TC7_CH0 -#define TC7_CH0 NOT_ON_TIMER -#endif -#ifndef TC7_CH1 -#define TC7_CH1 NOT_ON_TIMER -#endif - - -/* - * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices - * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway? - * - * Note: only the pins which have timers associated are listed in this table. - * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table. - * - * See Microchip Technology datasheet DS40001882F-page 30 - */ -struct wo_association WO_associations[] = { - - { PORTA, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, - { PORTA, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, - { PORTA, 2, NOT_ON_TIMER, 0, TCC3_CH0, 0}, - { PORTA, 3, NOT_ON_TIMER, 0, TCC3_CH1, 1}, - // PB04, PB05, PB06, PB07 - no timers - { PORTB, 8, TC4_CH0, 0, TCC3_CH6, 6}, - { PORTB, 9, TC4_CH1, 1, TCC3_CH7, 7}, - { PORTA, 4, TCC0_CH0, 0, TCC3_CH2, 2}, - { PORTA, 5, TCC0_CH1, 1, TCC3_CH3, 3}, - { PORTA, 6, TCC1_CH0, 0, TCC3_CH4, 4}, - { PORTA, 7, TCC1_CH1, 1, TCC3_CH5, 5}, - { PORTA, 8, TCC0_CH0, 0, TCC1_CH2, 2}, - { PORTA, 9, TCC0_CH1, 1, TCC1_CH3, 3}, - { PORTA, 10, TCC1_CH0, 0, TCC0_CH2, 2}, - { PORTA, 11, TCC1_CH1, 1, TCC0_CH3, 3}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4}, - { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5}, - { PORTB, 12, TC4_CH0, 0, TCC0_CH6, 6}, - { PORTB, 13, TC4_CH1, 1, TCC0_CH7, 7}, - { PORTB, 14, TC5_CH0, 0, NOT_ON_TIMER, 0}, - { PORTB, 15, TC5_CH1, 1, NOT_ON_TIMER, 0}, - { PORTA, 12, TCC2_CH0, 0, TCC0_CH6, 6}, - { PORTA, 13, TCC2_CH1, 1, TCC0_CH7, 7}, - { PORTA, 14, TC3_CH0, 0, TCC0_CH4, 4}, - { PORTA, 15, TC3_CH1, 1, TCC0_CH5, 5}, - { PORTA, 16, TCC2_CH0, 0, TCC0_CH6, 6}, - { PORTA, 17, TCC2_CH1, 1, TCC0_CH7, 7}, - { PORTA, 18, TC3_CH0, 0, TCC0_CH2, 2}, - { PORTA, 19, TC3_CH1, 1, TCC0_CH3, 3}, - { PORTB, 16, TC6_CH0, 0, TCC0_CH4, 4}, - { PORTB, 17, TC6_CH1, 1, TCC0_CH5, 5}, - { PORTA, 20, TC7_CH0, 0, TCC0_CH6, 6}, - { PORTA, 21, TC7_CH1, 1, TCC0_CH7, 7}, - { PORTA, 22, TC4_CH0, 0, TCC0_CH4, 4}, - { PORTA, 23, TC4_CH1, 1, TCC0_CH5, 5}, - { PORTA, 24, TC5_CH0, 0, TCC1_CH2, 2}, - { PORTA, 25, TC5_CH1, 1, TCC1_CH3, 3}, - { PORTB, 22, TC7_CH0, 0, TCC3_CH0, 0}, - { PORTB, 23, TC7_CH1, 1, TCC3_CH1, 1}, - { PORTA, 27, NOT_ON_TIMER, 0, TCC3_CH6, 6}, - { PORTA, 28, NOT_ON_TIMER, 0, TCC3_CH7, 7}, - { PORTA, 30, TCC1_CH0, 0, TCC3_CH4, 4}, - { PORTA, 31, TCC1_CH1, 1, TCC3_CH5, 5}, - { PORTB, 30, TCC0_CH0, 0, TCC1_CH2, 2}, - { PORTB, 31, TCC0_CH1, 1, TCC1_CH3, 3}, - { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0}, - { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 2, TC6_CH0, 0, TCC3_CH2, 2}, - { PORTB, 3, TC6_CH1, 1, TCC3_CH3, 3} -}; -#define NUM_WO_ASSOCIATIONS 48 - -wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; - - -struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { - for (int i=0;i no timers + { PORTA, 8, TC0_CH0, 0, TCC0_CH0, 0, TCC1_CH0, 4}, + { 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, 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}, + { PORTB, 15, TC5_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 3}, + { 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}, //? + { 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}, + { 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}, + { 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}, + { 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, 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 + { PORTB, 21, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTA, 20, TC7_CH0, 0, TCC1_CH0, 4, TCC0_CH0, 0}, + { PORTA, 21, TC7_CH1, 1, TCC1_CH1, 5, TCC0_CH1, 1}, + { PORTA, 22, TC4_CH0, 0, TCC1_CH2, 6, TCC0_CH2, 2}, + { PORTA, 23, TC4_CH1, 1, TCC1_CH3, 7, TCC0_CH3, 3}, + { PORTA, 24, TC5_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, // PDEC0 + { PORTA, 25, TC5_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 22, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 23, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC0 + { PORTB, 24, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 25, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, + { PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0}, + { PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0}, + // 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}, + // 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}, + { PORTB, 2, TC6_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, + +}; + +wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; + +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; + +struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { + for (int i=0;i>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; +} + + + +void syncTCC(Tcc* TCCx) { + while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); +} + + + + +void writeSAMDDutyCycle(int chaninfo, float dc) { + uint8_t tccn = GetTCNumber(chaninfo); + uint8_t chan = GetTCChannelNumber(chaninfo); + if (tccnCCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! + tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); + + tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); + while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); + } + else { + // Tc* tc = (Tc*)GetTC(chaninfo); + // //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + // tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + } +} + + +#define DPLL_CLOCK_NUM 2 // use GCLK2 +#define PWM_CLOCK_NUM 3 // use GCLK3 + + +/** + * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that + * any compatible pin combination can be used without having to worry about configuring different + * clocks. + */ +void configureSAMDClock() { + if (!SAMDClockConfigured) { + SAMDClockConfigured = true; // mark clock as configured + for (int i=0;iGENCTRL[DPLL_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) + // | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + // while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<PCHCTRL[1].reg = GCLK_PCHCTRL_GEN(DPLL_CLOCK_NUM)|GCLK_PCHCTRL_CHEN; + // while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<Dpll[0].DPLLCTRLA.bit.ENABLE = 0; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + // OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = OSCCTRL_DPLLCTRLB_REFCLK_GCLK_Val; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + // OSCCTRL->Dpll[0].DPLLRATIO.reg = 3; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + // OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 1; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + GCLK->GENCTRL[PWM_CLOCK_NUM].bit.GENEN = 0; + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC + | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<PCHCTRL[GCLK_CLKCTRL_ID_ofthistcc].reg = GCLK_PCHCTRL_GEN(PWM_CLOCK_NUM)|GCLK_PCHCTRL_CHEN; + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<CTRLA.bit.ENABLE = 0; //switch off tcc + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync + + uint8_t invenMask = ~(1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; + syncTCC(tcc); // wait for sync + + if (hw6pwm>0.0) { + tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + syncTCC(tcc); // wait for sync + } + + if (!tccConfigured[tccConfig.tcc.tccn]) { + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this? + while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync + + tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync + + // set all channels to 0% + for (int i=0;iCC[i].reg = 0; // start off at 0% duty cycle + uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i); + while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + } + } + + // Enable TCC + tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync + +#ifdef SIMPLEFOC_SAMD_DEBUG + 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("]"); +#endif + } + else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { + Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); + + // disable + // tc->COUNT8.CTRLA.bit.ENABLE = 0; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // unfortunately we need the 8-bit counter mode to use the PER register... + // tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000... + // tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // period is 250, period cannot be higher than 256! + // tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // initial duty cycle is 0 + // tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // enable + // 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); + #endif + } + + // set as configured + tccConfigured[tccConfig.tcc.tccn] = true; + + +} + + + + + +#endif diff --git a/src/drivers/hardware_specific/samd51_wo_associations.h b/src/drivers/hardware_specific/samd51_wo_associations.h deleted file mode 100644 index b59f13bb..00000000 --- a/src/drivers/hardware_specific/samd51_wo_associations.h +++ /dev/null @@ -1,124 +0,0 @@ - - - -// TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation -// 0 6 8 24-bit Yes Yes Yes Yes Yes Yes -// 1 4 8 24-bit Yes Yes Yes Yes Yes Yes -// 2 3 3 16-bit Yes - Yes - - - -// 3 2 2 16-bit Yes - - - - - -// 4 2 2 16-bit Yes - - - - - - - -#include "variant.h" - - -#ifdef _SAMD51_ - - - -struct wo_association { - EPortType port; - uint32_t pin; - ETCChannel tccE; - uint8_t woE; - ETCChannel tccF; - uint8_t woF; - ETCChannel tccG; - uint8_t woG; -}; - - -struct wo_association WO_associations[] = { - - { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, - { PORTA, 4, TC0_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, - { PORTA, 5, TC0_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, - { PORTA, 6, TC1_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, - { PORTA, 7, TC1_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, - { PORTC, 4, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, - // PC05, PC06, PC07 -> no timers - { PORTA, 8, TC0_CH0, 0, TCC0_CH0, 0, TCC1_CH0, 4}, - { 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, 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}, - { PORTB, 15, TC5_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 3}, - { 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}, //? - { 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}, - { 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}, - { 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}, - { 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, 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 - { PORTB, 21, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, - { PORTA, 20, TC7_CH0, 0, TCC1_CH0, 4, TCC0_CH0, 0}, - { PORTA, 21, TC7_CH1, 1, TCC1_CH1, 5, TCC0_CH1, 1}, - { PORTA, 22, TC4_CH0, 0, TCC1_CH2, 6, TCC0_CH2, 2}, - { PORTA, 23, TC4_CH1, 1, TCC1_CH3, 7, TCC0_CH3, 3}, - { PORTA, 24, TC5_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, // PDEC0 - { PORTA, 25, TC5_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 - { PORTB, 22, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 - { PORTB, 23, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC0 - { PORTB, 24, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 - { PORTB, 25, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 - { PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, - { PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, - { PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0}, - { PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0}, - // 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}, - // 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}, - { PORTB, 2, TC6_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, - -}; -#define NUM_WO_ASSOCIATIONS 72 - -wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; - - -struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { - for (int i=0;iSYNCBUSY.bit.CTRLB); // or while (TCC0->SYNCBUSY.reg); -//int count = TCC0->COUNT.reg; - - -/** - * Prints a table of pin assignments for your SAMD MCU. Very useful since the - * board pinout descriptions and variant.cpp are usually quite wrong, and this - * saves you hours of cross-referencing with the datasheet. - */ -void printAllPinInfos() { - Serial.println(); - for (uint8_t pin=0;pin=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); - } - else - Serial.println(" None"); -} - - - diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..92b0b239 --- /dev/null +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -0,0 +1,862 @@ + + + +#include "./samd_mcu.h" + +#if defined(_SAMD21_)||defined(_SAMD51_) + + + +/** + * Global state + */ +tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; +uint8_t numTccPinConfigurations = 0; +bool SAMDClockConfigured = false; +bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; + + + + + +/** + * Attach the TCC to the pin + */ +bool attachTCC(tccConfiguration& tccConfig) { + if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS) + return false; + pinMode(tccConfig.pin, OUTPUT); + + pinPeripheral(tccConfig.pin, tccConfig.peripheral); + tccPinConfigurations[numTccPinConfigurations++] = tccConfig; + return true; +} + + + + + +int getPermutationNumber(int pins) { + int num = 1; + for (int i=0;i=TCC_INST_NUM) + return false; + + if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan) + return false; + if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan) + return false; + if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan) + return false; + if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan) + return false; + + if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan) + return false; + if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo) + return false; + + return true; +} + + + + +bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) { + for (int i=0;i=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM + || pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM) + return false; + + // check we're not in use + if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl)) + return false; + + // check pins are all different tccs/channels + if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + + // check H/L pins are on same timer + if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn) + return false; + + // check H/L pins aren't on both the same timer and wo + if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo) + return false; + + return true; +} + + + + + +int checkHardware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + for (int i=0;i<64;i++) { + tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0)); + tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1)); + tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2)); + tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3)); + tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4)); + tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5)); + if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) + return i; + } + return -1; +} + + + + +int checkSoftware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + for (int i=0;i<64;i++) { + tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0)); + tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1)); + tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2)); + tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3)); + tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4)); + tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5)); + if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) + return i; + } + return -1; +} + + + +int scorePermutation(tccConfiguration pins[], uint8_t num) { + uint32_t usedtccs = 0; + for (int i=0;i>1; + } + for (int i=0;i>1; + } + return score; +} + + + + + + + + +int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) { + tccConfiguration tccConfs[num]; + int best = -1; + int bestscore = 1000000; + for (int i=0;i<(0x1<tcc.chaninfo, dc_a); + tccI = getTccPinConfiguration(pinB); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + return; +} + + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { + tccConfiguration* tccI = getTccPinConfiguration(pinA); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); + tccI = getTccPinConfiguration(pinB); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + tccI = getTccPinConfiguration(pinC); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); + return; +} + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + tccConfiguration* tccI = getTccPinConfiguration(pin1A); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); + tccI = getTccPinConfiguration(pin2A); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); + tccI = getTccPinConfiguration(pin1B); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); + tccI = getTccPinConfiguration(pin2B); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); + return; +} + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored + * the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side + * duty cycle. + * No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method... + * so use appropriately. + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +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){ + tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); + tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); + 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 + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly + + tcc1 = getTccPinConfiguration(pinB_h); + tcc2 = getTccPinConfiguration(pinB_l); + if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { + float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + + tcc1 = getTccPinConfiguration(pinC_h); + tcc2 = getTccPinConfiguration(pinC_l); + if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { + float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + return; +} + + + + +#ifdef SIMPLEFOC_SAMD_DEBUG + +/** + * Prints a table of pin assignments for your SAMD MCU. Very useful since the + * board pinout descriptions and variant.cpp are usually quite wrong, and this + * saves you hours of cross-referencing with the datasheet. + */ +void printAllPinInfos() { + Serial.println(); + for (uint8_t pin=0;pin=TCC_INST_NUM) + Serial.print(": TC Peripheral"); + else + Serial.print(": TCC Peripheral"); + switch (info.peripheral) { + case PIO_TIMER: + Serial.print(" E "); break; + case PIO_TIMER_ALT: + Serial.print(" F "); break; +#ifdef _SAMD51_ + case PIO_TCC_PDEC: + Serial.print(" G "); break; +#endif + default: + 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("]"); + } + else + Serial.println(" None"); +} + + + +#endif + +#endif + + diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h new file mode 100644 index 00000000..be07e0ba --- /dev/null +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -0,0 +1,103 @@ + +#ifndef SAMD_MCU_H +#define SAMD_MCU_H + + +// uncomment to enable debug output to Serial port +#define SIMPLEFOC_SAMD_DEBUG + + + +#include "Arduino.h" +#include "variant.h" +#include "wiring_private.h" +#include "../hardware_api.h" + + + + +#if defined(_SAMD21_)||defined(_SAMD51_) + + +#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION +#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 +#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 +#endif + +#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS +#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 +#endif + + + +struct tccConfiguration { + uint8_t pin; + EPioType peripheral; // 1=true, 0=false + uint8_t wo; + union tccChanInfo { + struct { + int8_t chan; + int8_t tccn; + }; + uint16_t chaninfo; + } tcc; +}; + + + + + + +struct wo_association { + EPortType port; + uint32_t pin; + ETCChannel tccE; + uint8_t woE; + ETCChannel tccF; + uint8_t woF; +#if defined(_SAMD51_) + ETCChannel tccG; + uint8_t woG; +#endif +}; + + + +#if defined(_SAMD21_) +#define NUM_PIO_TIMER_PERIPHERALS 2 +#elif defined(_SAMD51_) +#define NUM_PIO_TIMER_PERIPHERALS 3 +#endif + + + +/** + * Global state + */ +extern struct wo_association WO_associations[]; +extern uint8_t TCC_CHANNEL_COUNT[]; +extern tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; +extern uint8_t numTccPinConfigurations; +extern bool SAMDClockConfigured; +extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; + + + +struct wo_association& getWOAssociation(EPortType port, uint32_t pin); +void writeSAMDDutyCycle(int chaninfo, float dc); +void configureSAMDClock(); +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1); +__inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); +EPioType getPeripheralOfPermutation(int permutation, int pin_position); + +#ifdef SIMPLEFOC_SAMD_DEBUG +void printTCCConfiguration(tccConfiguration& info); +void printAllPinInfos(); +#endif + + + +#endif + + +#endif From eed0387916249bcab7c40eb7b78e8331f188b85f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 9 Apr 2021 18:32:10 +0200 Subject: [PATCH 05/45] prepare readme for 2.1.1 --- README.md | 38 ++++---------------------------------- library.properties | 2 +- 2 files changed, 5 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index e1ae2980..364e00a9 100644 --- a/README.md +++ b/README.md @@ -16,40 +16,10 @@ 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) -##### NEW RELEASE 📢: SimpleFOClibrary v2.1 -> - **Initial current sensing support**🎉 -> - Inline current sensors -> - adaptive zero finding and shunt direction -> - **Implemented real torque control** -> - using voltage -> - using current magnitude (one current) -> - using FOC currents ( d-q currents ) - real foc control -> - SVPWM full implementation d+q axis -> - **Simplified sensor implementation**📢 -> - For new sensor implementation only one function necessary `getAngle()` -> - Upgrade of the HallSensor implementation by [@owennewo](https://github.com/owennewo) -> - Support for Arduino DUE - everything except the 6PWM mode -> - Support for ATMega328pb -> - bugfix for the Teensy boards ( setting 3pwm ) -> - extended support for 2PWM stepper drivers - by [@zjor](https://github.com/zjor) -> - included F macro for shrinking string memory usage - moved to programming memory -> - disable phase support for 3pwm driver -> - not yet for 6pwm -> - rewritten `initFOC()` -> - can be skipped and outputs much more info -> - align sensor: direction + zero offset + pole pair check -> - align current sense -> - sensor offset supported (`motor.sensor_offset`) -> - **refactored motor commands interface** -> - much more flexible and easy to extend -> - very easy to add new commands and function callbacks -> - implemented motor+pid+lpf commands of-the-shelf -> - Added **step/dir interface** -> - integrated as an optional communication channel -> -> BEWARE 📢 slight API changes included -> - `ControlType` renamed into `MotionControlType` -> - `ControlType::voltage` does not exist any more now - `MotionControlType::torque` +##### Next release will be: SimpleFOClibrary v2.1.1 +> - bugfixes commander +> - bugfix `voltage_limit` when provided `phase_resistance` +> - bugfix `hall_sensor` examples ## Arduino *SimpleFOCShield* v2.0.3 diff --git a/library.properties b/library.properties index 062ff725..af8559f1 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1 +version=2.1.1 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From c4ff6e1af9891410e580d7e530efb0ae15216b2d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 10 Apr 2021 00:37:58 +0200 Subject: [PATCH 06/45] refactored code. SAMD51 initial support working --- src/drivers/hardware_specific/samd51_mcu.cpp | 1 - src/drivers/hardware_specific/samd_mcu.cpp | 22 +++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 4e33f9d4..df025a13 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -133,7 +133,6 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); - tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 92b0b239..c30b4a90 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -769,15 +769,17 @@ void printAllPinInfos() { if (association.tccE>=0) { int tcn = GetTCNumber(association.tccE); if (tcn>=TCC_INST_NUM) - Serial.print(" TC"); + Serial.print(" TC"); else - Serial.print(" TCC"); + Serial.print("TCC"); Serial.print(tcn); Serial.print("-"); Serial.print(GetTCChannelNumber(association.tccE)); Serial.print("["); Serial.print(GetTCChannelNumber(association.woE)); Serial.print("]"); + if (tcn<10) + Serial.print(" "); } else Serial.print(" None "); @@ -786,24 +788,28 @@ void printAllPinInfos() { if (association.tccF>=0) { int tcn = GetTCNumber(association.tccF); if (tcn>=TCC_INST_NUM) - Serial.print(" TC"); + Serial.print(" TC"); else - Serial.print(" TCC"); + Serial.print("TCC"); Serial.print(tcn); Serial.print("-"); Serial.print(GetTCChannelNumber(association.tccF)); Serial.print("["); Serial.print(GetTCChannelNumber(association.woF)); - Serial.println("]"); + Serial.print("]"); + if (tcn<10) + Serial.print(" "); } else - Serial.println(" None "); + Serial.print(" None "); #ifdef _SAMD51_ Serial.print(" G="); if (association.tccG>=0) { int tcn = GetTCNumber(association.tccG); - Serial.print(" TCC"); + Serial.print("TCC"); + if (tcn<10) + Serial.print(" "); Serial.print(tcn); Serial.print("-"); Serial.print(GetTCChannelNumber(association.tccG)); @@ -813,6 +819,8 @@ void printAllPinInfos() { } else Serial.println(" None "); +#else + Serial.println(""); #endif } From eaf37ca1d5e48d4715b757744b6e213593cd8ca9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 10 Apr 2021 16:10:55 +0200 Subject: [PATCH 07/45] magnetic pwm sensors initial implementation --- .../find_raw_min_max/find_raw_min_max.ino | 30 +++++++++++++++ .../magnetic_sensor_pwm.ino | 32 ++++++++++++++++ ...magnetic_sensor_pwm_software_interrupt.ino | 38 +++++++++++++++++++ keywords.txt | 3 ++ src/sensors/MagneticSensorPWM.cpp | 33 +++++++++++++++- src/sensors/MagneticSensorPWM.h | 10 ++++- 6 files changed, 143 insertions(+), 3 deletions(-) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino 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_example/find_raw_min_max/find_raw_min_max.ino new file mode 100644 index 00000000..50dcccae --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino @@ -0,0 +1,30 @@ +#include + + +/** + * An example to find out the raw max and min count to be provided to the constructor + * SPin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value + * And replace values 4 and 904 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle(). + * If there is a jump that means you can still find better values. + */ +MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904); +void doPWM(){sensor.handlePWM();} + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + sensor.init(); + sensor.enableInterrupt(doPWM); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // display the angle and the angular velocity to the terminal + Serial.print(sensor.pulse_length_us); + Serial.print("\t"); + Serial.println(sensor.getAngle()); +} 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_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino new file mode 100644 index 00000000..8d0bd54f --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino @@ -0,0 +1,32 @@ +#include + + +/** + * Magnetic sensor reading pwm signal on pin 2. The pwm duty cycle is proportional to the sensor angle. + * + * MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max) + * - pinPWM - the pin that is reading the pwm from magnetic sensor + * - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution + * - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation + */ +MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904); +void doPWM(){sensor.handlePWM();} + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + sensor.init(); + sensor.enableInterrupt(doPWM); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // 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_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino new file mode 100644 index 00000000..5d3b2b5a --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -0,0 +1,38 @@ +#include + +// software interrupt library +#include +#include + +/** + * Magnetic sensor reading analog voltage on pin which does not have hardware interrupt support. Such as A0. + * + * MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max) + * - pinPWM - the pin that is reading the pwm from magnetic sensor + * - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution + * - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation + */ +MagneticSensorPWM sensor = MagneticSensorPWM(A0, 4, 904); +void doPWM(){sensor.handlePWM();} + +// encoder interrupt init +PciListenerImp listenerPWM(sensor.pinPWM, doPWM);} + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + sensor.init(); + PciManager.registerListener(&listenerPWM); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} diff --git a/keywords.txt b/keywords.txt index 9ec297c8..9888da34 100644 --- a/keywords.txt +++ b/keywords.txt @@ -8,6 +8,7 @@ HallSensors KEYWORD1 MagneticSensorSPI KEYWORD1 MagneticSensorI2C KEYWORD1 MagneticSensorAnalog KEYWORD1 +MagneticSensorPWM KEYWORD1 BLDCDriver3PWM KEYWORD1 BLDCDriver6PWM KEYWORD1 BLDCDriver KEYWORD1 @@ -92,6 +93,8 @@ scalar KEYWORD2 pid KEYWORD2 lpf KEYWORD2 motor KEYWORD2 +handlePWM KEYWORD2 +enableInterrupt KEYWORD2 diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 7ae989be..4e9f944e 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -1,4 +1,5 @@ #include "MagneticSensorPWM.h" +#include "Arduino.h" /** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max) * @param _pinPWM the pin that is reading the pwm from magnetic sensor @@ -14,6 +15,9 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m max_raw_count = _max_raw_count; pinMode(pinPWM, INPUT); + + // init last call + last_call_us = _micros(); } @@ -57,10 +61,35 @@ float MagneticSensorPWM::getAngle(){ // get velocity (rad/s) float MagneticSensorPWM::getVelocity(){ - return velocity; + return velocity; } // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ - return pulseIn(pinPWM,HIGH); +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 + pulse_length_us = pulseIn(pinPWM, HIGH); +#endif + + return pulse_length_us; +} + + +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; + + // save the currrent timestamp for the next call + last_call_us = now_us; } + +// function enabling hardware interrupts of the for the callback provided +// if callback is not provided then the interrupt is not enabled +void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){ + #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 +} \ No newline at end of file diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 2957bcff..46053ac7 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -26,7 +26,11 @@ class MagneticSensorPWM: public Sensor{ float getAngle() override; // get current angular velocity (rad/s) float getVelocity() override; - + + // pwm handler + void handlePWM(); + void enableInterrupt(void (*doPWM)()); + unsigned long pulse_length_us; private: // raw count (typically in range of 0-1023) @@ -50,6 +54,10 @@ class MagneticSensorPWM: public Sensor{ float angle_prev; //!< angle in previous velocity calculation step long velocity_calc_timestamp; //!< last velocity calculation timestamp float velocity; + + // time tracking variables + unsigned long last_call_us; + // unsigned long pulse_length_us; }; From 273bcf8f70c7e0097040b4f864156285b25437eb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 22 Apr 2021 03:42:03 +0200 Subject: [PATCH 08/45] fix compile problem on AVR due to SAMD header --- src/drivers/hardware_specific/samd_mcu.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index be07e0ba..9a0d3b05 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -6,17 +6,15 @@ // uncomment to enable debug output to Serial port #define SIMPLEFOC_SAMD_DEBUG - - -#include "Arduino.h" -#include "variant.h" -#include "wiring_private.h" #include "../hardware_api.h" +#if defined(_SAMD21_)||defined(_SAMD51_) -#if defined(_SAMD21_)||defined(_SAMD51_) +#include "Arduino.h" +#include "variant.h" +#include "wiring_private.h" #ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION From dce79065d962d39d39c7fef1c872b8abce166d49 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 24 Apr 2021 18:11:23 +0200 Subject: [PATCH 09/45] extension of the commander, + typo fix in pwm mag sensor --- README.md | 3 + ...magnetic_sensor_pwm_software_interrupt.ino | 2 +- keywords.txt | 2 + src/common/base_classes/FOCMotor.h | 2 +- src/common/defaults.h | 2 +- src/common/pid.cpp | 16 +++--- src/communication/Commander.cpp | 56 +++++++++++++++++-- src/communication/commands.h | 7 ++- 8 files changed, 75 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 364e00a9..3e2e0a7f 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,9 @@ Therefore this is an attempt to: > - bugfixes commander > - bugfix `voltage_limit` when provided `phase_resistance` > - bugfix `hall_sensor` examples +> - added examples fot the powershield +> - added initial support for `MagneticSensorPWM` +> - extension of the `Commander` interface ## Arduino *SimpleFOCShield* v2.0.3 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_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino index 5d3b2b5a..5b7665a0 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_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -16,7 +16,7 @@ MagneticSensorPWM sensor = MagneticSensorPWM(A0, 4, 904); void doPWM(){sensor.handlePWM();} // encoder interrupt init -PciListenerImp listenerPWM(sensor.pinPWM, doPWM);} +PciListenerImp listenerPWM(sensor.pinPWM, doPWM); void setup() { // monitoring port diff --git a/keywords.txt b/keywords.txt index 9888da34..971a3ffb 100644 --- a/keywords.txt +++ b/keywords.txt @@ -122,6 +122,8 @@ sensor_offset KEYWORD2 zero_electric_angle KEYWORD2 verbose KEYWORD2 decimal_places KEYWORD2 +phase_resistance KEYWORD2 +modulation_centered KEYWORD2 diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c918b052..37c5b260 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -169,7 +169,7 @@ class FOCMotor LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!< parameter determining the current Low pass filter configuration LowPassFilter LPF_current_d{DEF_CURR_FILTER_Tf};//!< parameter determining the current Low pass filter configuration PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration - PIDController P_angle{DEF_P_ANGLE_P,0,0,1e10,DEF_VEL_LIM}; //!< parameter determining the position PID configuration + PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM}; //!< parameter determining the position PID configuration LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!< parameter determining the velocity Low pass filter configuration LowPassFilter LPF_angle{0.0};//!< parameter determining the angle low pass filter configuration unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad diff --git a/src/common/defaults.h b/src/common/defaults.h index 9c8cee3a..71f39098 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -23,7 +23,7 @@ #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_RAMP 1e11 //!< default PID controller voltage ramp 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 #endif diff --git a/src/common/pid.cpp b/src/common/pid.cpp index a910556f..5290d814 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -40,13 +40,15 @@ float PIDController::operator() (float error){ // antiwindup - limit the output variable output = _constrain(output, -limit, limit); - // limit the acceleration by ramping the output - float output_rate = (output - output_prev)/Ts; - if (output_rate > output_ramp) - output = output_prev + output_ramp*Ts; - else if (output_rate < -output_ramp) - output = output_prev - output_ramp*Ts; - + // if output ramp defined + if(output_ramp > 0){ + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + } // saving for the next pass integral_prev = integral; output_prev = output; diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 6ee08d07..fae5165f 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -170,10 +170,10 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MOTION_TYPE: - printVerbose(F("Motion: ")); + printVerbose(F("Motion:")); switch(sub_cmd){ case SCMD_DOWNSAMPLE: - printVerbose(F("downsample: ")); + printVerbose(F(" downsample: ")); if(!GET) motor->motion_downsample = value; println((int)motor->motion_downsample); break; @@ -224,6 +224,38 @@ void Commander::motor(FOCMotor* motor, char* user_command) { if(!GET) (bool)value ? motor->enable() : motor->disable(); println(motor->enabled); break; + case CMD_PWMMOD: + // PWM modulation change + printVerbose(F("PWM Mod | ")); + switch (sub_cmd){ + case SCMD_PWMMOD_TYPE: // zero offset + printVerbose(F("type: ")); + if(!GET) motor->foc_modulation = (FOCModulationType)value; + switch(motor->foc_modulation){ + case FOCModulationType::SinePWM: + println(F("SinePWM")); + break; + case FOCModulationType::SpaceVectorPWM: + println(F("SVPWM")); + break; + case FOCModulationType::Trapezoid_120: + println(F("Trap 120")); + break; + case FOCModulationType::Trapezoid_150: + println(F("Trap 150")); + break; + } + break; + case SCMD_PWMMOD_CENTER: // centered modulation + printVerbose(F("center: ")); + if(!GET) motor->modulation_centered = value; + println(motor->modulation_centered); + break; + default: + printError(); + break; + } + break; case CMD_RESIST: // enable/disable printVerbose(F("R phase: ")); @@ -271,7 +303,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case 2: // get voltage d printVerbose(F("Vd: ")); - println(motor->voltage.q); + println(motor->voltage.d); break; case 3: // get current q printVerbose(F("Cq: ")); @@ -279,7 +311,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case 4: // get current d printVerbose(F("Cd: ")); - println(motor->current.q); + println(motor->current.d); break; case 5: // get velocity printVerbose(F("vel: ")); @@ -289,6 +321,22 @@ void Commander::motor(FOCMotor* motor, char* user_command) { printVerbose(F("angle: ")); println(motor->shaft_angle); break; + case 7: // get all states + printVerbose(F("all: ")); + print(motor->target); + print(";"); + print(motor->voltage.q); + print(";"); + print(motor->voltage.d); + print(";"); + print(motor->current.q); + print(";"); + print(motor->current.d); + print(";"); + print(motor->shaft_velocity); + print(";"); + println(motor->shaft_angle); + break; default: printError(); break; diff --git a/src/communication/commands.h b/src/communication/commands.h index 69405629..3ba6cdde 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -15,6 +15,7 @@ #define CMD_SENSOR 'S' //!< sensor offsets #define CMD_MONITOR 'M' //!< monitoring #define CMD_RESIST 'R' //!< motor phase resistance + #define CMD_PWMMOD 'W' //!< pwm modulation // commander configuration #define CMD_SCAN '?' //!< command scaning the network - only for commander @@ -41,5 +42,9 @@ #define SCMD_CLEAR 'C' //!< Clear all monitored variables #define SCMD_GET 'G' //!< Get variable only one value #define SCMD_SET 'S' //!< Set variables to be monitored - + + #define SCMD_PWMMOD_TYPE 'T' //!<< Pwm modulation type + #define SCMD_PWMMOD_CENTER 'C' //!<< Pwm modulation center flag + + #endif \ No newline at end of file From 2f3b5ff2ecb47042e5e991cc9f7b3f14dfb44d9b Mon Sep 17 00:00:00 2001 From: tschundler Date: Sat, 24 Apr 2021 19:28:27 -0700 Subject: [PATCH 10/45] Avoid the need for a custom header file for ESP32 compilation. Can be reverted when https://github.com/espressif/esp-idf/issues/5429 is resolved. --- src/drivers/hardware_specific/esp32_mcu.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 7350316d..cb839704 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -155,9 +155,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); _delay(1); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined + // in the default Espressif MCPWM headers. The correct const may be used + // when https://github.com/espressif/esp-idf/issues/5429 is resolved. + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, (mcpwm_sync_signal_t)1, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, (mcpwm_sync_signal_t)1, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, (mcpwm_sync_signal_t)1, 0); _delay(1); mcpwm_num->timer[0].sync.out_sel = 1; _delay(1); @@ -428,4 +431,4 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i } } } -#endif \ No newline at end of file +#endif From fd78d2ed70d9969f5fb97d709b393a46735ef760 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 28 Apr 2021 16:44:47 +0200 Subject: [PATCH 11/45] Fix consistency error in calibrateOffsets. --- src/current_sense/InlineCurrentSense.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index d5d10881..2e9c4421 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -28,21 +28,23 @@ void InlineCurrentSense::init(){ } // Function finding zero offsets of the ADC void InlineCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + // find adc offset = zero current voltage - offset_ia =0; - offset_ib= 0; - offset_ic= 0; + 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 += _readADCVoltage(pinA); offset_ib += _readADCVoltage(pinB); if(_isset(pinC)) offset_ic += _readADCVoltage(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 / 500.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) From 7bacc20a63f574978676bb925f25369e928165d4 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 28 Apr 2021 17:02:12 +0200 Subject: [PATCH 12/45] CurrentSense::gain_x must be a float --- src/current_sense/InlineCurrentSense.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 64c658d1..da46d104 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -29,9 +29,9 @@ class InlineCurrentSense: public CurrentSense{ // ADC measuremnet gain for each phase // support for different gains for different phases of more commonly - inverted phase currents // this should be automated later - int gain_a; //!< phase A gain - int gain_b; //!< phase B gain - int gain_c; //!< phase C gain + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain private: From dd73d88a0e07b387877258d1a098902120fc1870 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Thu, 29 Apr 2021 12:06:10 +0200 Subject: [PATCH 13/45] CurrentSense correct filtering if 3 current measurements are available --- src/common/base_classes/CurrentSense.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index accaebb3..2cec8dd5 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -17,8 +17,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; }else{ - i_alpha = 2*(current.a - (current.b - current.c))/3.0; - i_beta = _2_SQRT3 *( current.b - current.c ); + // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. + float mid = (1.f/3) * (current.a + current.b + current.c); + float a = current.a - mid; + float b = current.b - mid; + i_alpha = a; + i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } // if motor angle provided function returns signed value of the current @@ -44,9 +48,13 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }else{ - i_alpha = 0.6666667*(current.a - (current.b - current.c)); - i_beta = _2_SQRT3 *( current.b - current.c ); + } else { + // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. + float mid = (1.f/3) * (current.a + current.b + current.c); + float a = current.a - mid; + float b = current.b - mid; + i_alpha = a; + i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } // calculate park transform From f2ae008872e4db627cde67be2afd71c5a10c0d42 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 29 Apr 2021 23:43:24 +0200 Subject: [PATCH 14/45] added an enable_active_high field (default true) to BLDCDriver6PWM this is to use the enable pin on the DRV8316, which is actually a driver_off pin, and therefore active-low for enabled state --- src/drivers/BLDCDriver6PWM.cpp | 6 +++--- src/drivers/BLDCDriver6PWM.h | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 013e6415..f24cef9e 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,7 +24,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, HIGH); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high?HIGH:LOW); // set zero to PWM setPwm(0, 0, 0); } @@ -35,7 +35,7 @@ void BLDCDriver6PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, LOW); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high?LOW:HIGH); } @@ -82,4 +82,4 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling -} \ No newline at end of file +} diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index 68487370..cee37d64 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,6 +37,7 @@ class BLDCDriver6PWM: public BLDCDriver int pwmB_h,pwmB_l; //!< phase B pwm pin number int pwmC_h,pwmC_l; //!< phase C pwm pin number int enable_pin; //!< enable pin number + bool enable_active_high = true; float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] From 8ecc1b62a9387f5acee9ecabf038802339dfa11b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Apr 2021 22:43:15 +0200 Subject: [PATCH 15/45] fixing SAMD21 PWM - now it's really nice... --- src/drivers/hardware_specific/samd21_mcu.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 0c56eef4..cf8db836 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -310,17 +310,18 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t chan = GetTCChannelNumber(chaninfo); if (tccnCC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - //uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); - //while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + // set via CC +// tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); +// uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); +// while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB + while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); - tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); - tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); - while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); +// tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); +// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); +// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { Tc* tc = (Tc*)GetTC(chaninfo); From 62534c2c7ba231161e8fc124cf3302e3d2bdea22 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 2 May 2021 16:36:52 +0200 Subject: [PATCH 16/45] optimized the enable_active_high setting for enable/disable --- src/drivers/BLDCDriver6PWM.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index f24cef9e..5dd99eb2 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,7 +24,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high?HIGH:LOW); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set zero to PWM setPwm(0, 0, 0); } @@ -35,7 +35,7 @@ void BLDCDriver6PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high?LOW:HIGH); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, !enable_active_high); } From c43f1e3a7a9e32081ded1f7d6d6489e3b222840a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 2 May 2021 16:39:26 +0200 Subject: [PATCH 17/45] fix samd51 pwm glitch and enable same51 support --- src/drivers/hardware_specific/generic_mcu.cpp | 6 ++++-- src/drivers/hardware_specific/samd51_mcu.cpp | 13 +++++++------ src/drivers/hardware_specific/samd_mcu.cpp | 10 +++++----- src/drivers/hardware_specific/samd_mcu.h | 13 ++++++++++--- 4 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 170b9dae..fd64e6af 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -12,9 +12,11 @@ #elif defined(_STM32_DEF_) // or stm32 -#elif defined(_SAMD21_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD21_) // samd21 -#elif defined(_SAMD51_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD51_) // samd51 + +#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 #else diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index df025a13..69c44848 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -3,7 +3,7 @@ #include "./samd_mcu.h" -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) @@ -129,12 +129,13 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t chan = GetTCChannelNumber(chaninfo); if (tccnSYNCBUSY.vec.CC & (0x1< 0 ); tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! - tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); - tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); - while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); +// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); +// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { // Tc* tc = (Tc*)GetTC(chaninfo); diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index c30b4a90..fdbd7a7f 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -3,7 +3,7 @@ #include "./samd_mcu.h" -#if defined(_SAMD21_)||defined(_SAMD51_) +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) @@ -90,7 +90,7 @@ tccConfiguration getTCCChannelNr(int pin, EPioType peripheral) { result.tcc.chaninfo = association.tccF; result.wo = association.woF; } -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) else if (peripheral==PIO_TCC_PDEC) { result.tcc.chaninfo = association.tccG; result.wo = association.woG; @@ -758,7 +758,7 @@ void printAllPinInfos() { case PORTA: Serial.print(" PA"); break; case PORTB: Serial.print(" PB"); break; case PORTC: Serial.print(" PC"); break; -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) case PORTD: Serial.print(" PD"); break; #endif } @@ -803,7 +803,7 @@ void printAllPinInfos() { else Serial.print(" None "); -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) Serial.print(" G="); if (association.tccG>=0) { int tcn = GetTCNumber(association.tccG); @@ -842,7 +842,7 @@ void printTCCConfiguration(tccConfiguration& info) { Serial.print(" E "); break; case PIO_TIMER_ALT: Serial.print(" F "); break; -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: Serial.print(" G "); break; #endif diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 9a0d3b05..f0fd5460 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -9,7 +9,14 @@ #include "../hardware_api.h" -#if defined(_SAMD21_)||defined(_SAMD51_) +#if defined(__SAME51J19A__) || defined(__ATSAME51J19A__) +#ifndef _SAME51_ +#define _SAME51_ +#endif +#endif + + +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) #include "Arduino.h" @@ -53,7 +60,7 @@ struct wo_association { uint8_t woE; ETCChannel tccF; uint8_t woF; -#if defined(_SAMD51_) +#if defined(_SAMD51_)||defined(_SAME51_) ETCChannel tccG; uint8_t woG; #endif @@ -63,7 +70,7 @@ struct wo_association { #if defined(_SAMD21_) #define NUM_PIO_TIMER_PERIPHERALS 2 -#elif defined(_SAMD51_) +#elif defined(_SAMD51_)||defined(_SAME51_) #define NUM_PIO_TIMER_PERIPHERALS 3 #endif From ebd4906c51eed83ee8b44df3fd1d3aa4feef6b98 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 3 May 2021 00:55:37 +0200 Subject: [PATCH 18/45] move the sensor calls to before the enable checks in move and loopFOC this enables two things: 1. you can still see the sensors change and update in SimpleFOCStudio even if you disable the motor. This is useful to determine sensor accuracy etc. 2. you previously ran the risk of losing track of full rotations if the motor was (e.g. manually) moved while disabled. With this change we always track the sensor values and keep track of all rotations. --- src/BLDCMotor.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 76dc7032..bcc486e5 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -244,13 +244,15 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + + // shaft angle + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated + // if disabled do nothing if(!enabled) return; // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - shaft_angle = shaftAngle(); // electrical angle - need shaftAngle to be called first electrical_angle = electricalAngle(); @@ -295,6 +297,10 @@ void BLDCMotor::loopFOC() { // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + + // 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; // downsampling (optional) @@ -302,8 +308,6 @@ void BLDCMotor::move(float new_target) { motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; - // get angular velocity - shaft_velocity = shaftVelocity(); switch (controller) { case MotionControlType::torque: From 20e16d7f6e6bb9cf6c29ca1038787465273265cd Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 4 May 2021 20:19:09 +0200 Subject: [PATCH 19/45] FEAT added raw analog/pwm --- .../find_raw_min_max/find_raw_min_max.ino | 51 +++++++++++++++++++ .../magnetic_sensor_analog.ino} | 3 +- .../find_raw_min_max/find_raw_min_max.ino | 19 +++++-- src/drivers/hardware_specific/esp32_mcu.cpp | 13 +++-- src/sensors/MagneticSensorAnalog.h | 4 +- 5 files changed, 80 insertions(+), 10 deletions(-) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino rename examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/{magnetic_sensor_analog_example.ino => magnetic_sensor_analog/magnetic_sensor_analog.ino} (99%) 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_example/find_raw_min_max/find_raw_min_max.ino new file mode 100644 index 00000000..e45b0278 --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino @@ -0,0 +1,51 @@ +#include + +/** + * An example to find out the raw max and min count to be provided to the constructor + * Spin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value + * And replace values 14 and 1020 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle(). + * If there is a jump that means you can still find better values. + */ + +/** + * Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position. + * Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus. + * + * MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max) + * - pinAnalog - the pin that is reading the pwm from magnetic sensor + * - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~15. Getting this wrong results in a small click once per revolution + * - max_raw_count - the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC + */ +MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + sensor.init(); + + Serial.println("Sensor ready"); + _delay(1000); +} + +int max_count = 0; +int min_count = 100000; + +void loop() { + + // keep track of min and max + if(sensor.raw_count > max_count) max_count = sensor.raw_count; + else if(sensor.raw_count < min_count) min_count = sensor.raw_count; + + // display the raw count, and max and min raw count + Serial.print("angle:"); + Serial.print(sensor.getAngle()); + Serial.print("\t, raw:"); + Serial.print(sensor.raw_count); + Serial.print("\t, min:"); + Serial.print(min_count); + Serial.print("\t, max:"); + Serial.println(max_count); + delay(100); +} \ No newline at end of file diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino similarity index 99% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino index ef34127c..40d47ba8 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino @@ -1,6 +1,7 @@ #include + /** * Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position. * Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus. @@ -28,4 +29,4 @@ void loop() { Serial.print(sensor.getAngle()); Serial.print("\t"); Serial.println(sensor.getVelocity()); -} +} \ No newline at end of file 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_example/find_raw_min_max/find_raw_min_max.ino index 50dcccae..27816eec 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_example/find_raw_min_max/find_raw_min_max.ino @@ -22,9 +22,22 @@ void setup() { _delay(1000); } +int max_pulse= 0; +int min_pulse = 10000; + void loop() { - // display the angle and the angular velocity to the terminal + + // keep track of min and max + if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us; + else if(sensor.pulse_length_us < min_pulse) min_pulse = sensor.pulse_length_us; + + // display the raw count, and max and min raw count + Serial.print("angle:"); + Serial.print(sensor.getAngle()); + Serial.print("\t, raw:"); Serial.print(sensor.pulse_length_us); - Serial.print("\t"); - Serial.println(sensor.getAngle()); + Serial.print("\t, min:"); + Serial.print(min_pulse); + Serial.print("\t, max:"); + Serial.println(max_pulse); } diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 7350316d..bde9d5dd 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -154,10 +154,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); _delay(1); - - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + // mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + // mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + // mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined + // in the default Espressif MCPWM headers. The correct const may be used + // when https://github.com/espressif/esp-idf/issues/5429 is resolved. + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, (mcpwm_sync_signal_t)1, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, (mcpwm_sync_signal_t)1, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, (mcpwm_sync_signal_t)1, 0); _delay(1); mcpwm_num->timer[0].sync.out_sel = 1; _delay(1); diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 33d5b25c..12f78c76 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -33,10 +33,10 @@ class MagneticSensorAnalog: public Sensor{ /** get current angular velocity (rad/s) **/ float getVelocity() override; - - private: /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; + + private: int min_raw_count; int max_raw_count; int cpr; From 483c6f0e1161a4bada9346bf0af5b7cdc46c582a Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 4 May 2021 20:25:06 +0200 Subject: [PATCH 20/45] FEAT added readme updates with the new changes --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 3e2e0a7f..26c25c7d 100644 --- a/README.md +++ b/README.md @@ -20,9 +20,12 @@ Therefore this is an attempt to: > - bugfixes commander > - bugfix `voltage_limit` when provided `phase_resistance` > - bugfix `hall_sensor` examples -> - added examples fot the powershield +> - added examples fot the PowerShield > - added initial support for `MagneticSensorPWM` +> - added examples to find the raw max and min of the analog and pwm sensor > - extension of the `Commander` interface +> - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler + ## Arduino *SimpleFOCShield* v2.0.3 From f3988ba0d26333e2c4423dafeb4f09bc04831c98 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 4 May 2021 20:26:20 +0200 Subject: [PATCH 21/45] FEAT moved simplefocshield to the end --- README.md | 55 +++++++++++++++++++++++++++---------------------------- 1 file changed, 27 insertions(+), 28 deletions(-) diff --git a/README.md b/README.md index 26c25c7d..95524d16 100644 --- a/README.md +++ b/README.md @@ -26,34 +26,6 @@ Therefore this is an attempt to: > - extension of the `Commander` interface > - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler - - -## Arduino *SimpleFOCShield* v2.0.3 -

- - - -

- -### Features -- **Plug & play**: In combination with Arduino *Simple**FOC**library* - [github](https://github.com/simplefoc/Arduino-FOC) -- **Low-cost**: Price of €15 - [Check the pricing](https://www.simplefoc.com/shop) -- **In-line current sensing**: Up to 3Amps/5Amps bidirectional - - configurable: 3.3Amps - 3.3V adc, 5Amps - 5V adc -- **Integrated 8V regulator**: - - Enable/disable by soldering pads -- **Max power 120W** - max current 5A, power-supply 12-24V - - Designed for Gimbal motors with the internal resistance >10 Ωs. -- **Stackable**: running 2 motors in the same time -- **Encoder/Hall sensors interface**: Integrated 3.3kΩ pullups (configurable) -- **I2C interface**: Integrated 4.7kΩ pullups (configurable) -- **Configurable pinout**: Hardware configuration - soldering connections -- **Arduino headers**: Arduino UNO, Arduino MEGA, STM32 Nucleo boards... -- **Open Source**: Fully available fabrication files - [how to make it yourself](https://docs.simplefoc.com/arduino_simplefoc_shield_fabrication) - -

- - ## Arduino *SimpleFOClibrary* v2.1

@@ -84,6 +56,33 @@ This video demonstrates the *Simple**FOC**library* basic usage, electronic conne

+## Arduino *SimpleFOCShield* v2.0.3 +

+ + + +

+ +### Features +- **Plug & play**: In combination with Arduino *Simple**FOC**library* - [github](https://github.com/simplefoc/Arduino-FOC) +- **Low-cost**: Price of €15 - [Check the pricing](https://www.simplefoc.com/shop) +- **In-line current sensing**: Up to 3Amps/5Amps bidirectional + - configurable: 3.3Amps - 3.3V adc, 5Amps - 5V adc +- **Integrated 8V regulator**: + - Enable/disable by soldering pads +- **Max power 120W** - max current 5A, power-supply 12-24V + - Designed for Gimbal motors with the internal resistance >10 Ωs. +- **Stackable**: running 2 motors in the same time +- **Encoder/Hall sensors interface**: Integrated 3.3kΩ pullups (configurable) +- **I2C interface**: Integrated 4.7kΩ pullups (configurable) +- **Configurable pinout**: Hardware configuration - soldering connections +- **Arduino headers**: Arduino UNO, Arduino MEGA, STM32 Nucleo boards... +- **Open Source**: Fully available fabrication files - [how to make it yourself](https://docs.simplefoc.com/arduino_simplefoc_shield_fabrication) + +

+ + + ## Getting Started Depending on if you want to use this library as the plug and play Arduino library or you want to get insight in the algorithm and make changes there are two ways to install this code. From 1a37cadc2d6281d773f8fe0e918ecaf1f99555f9 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 4 May 2021 21:02:30 +0200 Subject: [PATCH 22/45] FEAT added the shaft_angle to the motor.initFOC() - issue #62 --- src/BLDCMotor.cpp | 7 +++++-- src/StepperMotor.cpp | 7 +++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 76dc7032..0291c4f2 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -98,8 +98,11 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); - if(sensor) exit_flag *= alignSensor(); - else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + if(sensor){ + exit_flag *= alignSensor(); + // added the shaft_angle update + shaft_angle = sensor->getAngle(); + }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 604914ed..9a7f1662 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -96,8 +96,11 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); - if(sensor) exit_flag = alignSensor(); - else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + if(sensor){ + exit_flag *= alignSensor(); + // 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.")); From 4aac99252ddb16c46a0d038a6831b5ff40b151b0 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 4 May 2021 21:03:36 +0200 Subject: [PATCH 23/45] added issue #62 --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 95524d16..04b33478 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,7 @@ Therefore this is an attempt to: > - added examples to find the raw max and min of the analog and pwm sensor > - extension of the `Commander` interface > - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler +> - issue #62: motor.shaft_angle setting in the motor.initFOC() ## Arduino *SimpleFOClibrary* v2.1 From 8ff4630bcff62c8e6cd2129ffd122922a0baa1cf Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 6 May 2021 18:43:33 +0200 Subject: [PATCH 24/45] FIX read sensor after the openloop check --- src/BLDCMotor.cpp | 5 ++--- src/StepperMotor.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index b33da117..9c2f89c0 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -247,14 +247,13 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { - + // if open-loop do nothing + if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; // shaft angle shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; - // if open-loop do nothing - if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; // electrical angle - need shaftAngle to be called first electrical_angle = electricalAngle(); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 9a7f1662..183c2c65 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -216,13 +216,14 @@ int StepperMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void StepperMotor::loopFOC() { - // if disabled do nothing - if(!enabled) return; // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle shaft_angle = shaftAngle(); + + // if disabled do nothing + if(!enabled) return; + electrical_angle = electricalAngle(); // set the phase voltage - FOC heart function :) @@ -235,6 +236,8 @@ 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) @@ -242,8 +245,6 @@ void StepperMotor::move(float new_target) { motion_cnt = 0; // set internal target variable if(_isset(new_target) ) target = new_target; - // get angular velocity - shaft_velocity = shaftVelocity(); // choose control loop switch (controller) { case MotionControlType::torque: From ce525351a417ecd39ace1cc0fd17858e904fadb0 Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 10:39:58 -0600 Subject: [PATCH 25/45] Issue #46: motor.command() and CRLF The eol character is now configurable The user can now actuvate echo feedback to see what he is typing I also removed a duplication in two run() overload, while fixing what I think was a mistake by enforcing the use of the provided argument Stream& even if the ctor one is not null. Lastly, I warn the user if \n is configured but \r is detected (only in user friendly mode) All the past code should work as before (no API change required) --- src/communication/Commander.cpp | 71 +++++++++++++++++++-------------- src/communication/Commander.h | 16 +++++--- 2 files changed, 53 insertions(+), 34 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 6ee08d07..e9672325 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -1,11 +1,14 @@ #include "Commander.h" -Commander::Commander(Stream& serial){ +Commander::Commander(Stream& serial, char eol, bool echo){ com_port = &serial; + this->eol = eol; + this->echo = echo; } -Commander::Commander(){ - // do nothing +Commander::Commander(char eol, bool echo){ + this->eol = eol; + this->echo = echo; } @@ -19,33 +22,24 @@ void Commander::add(char id, CommandCallback onCommand, char* label ){ void Commander::run(){ if(!com_port) return; - // a string to hold incoming data - while (com_port->available()) { - // get the new byte: - received_chars[rec_cnt] = (char)com_port->read(); - // end of user input - if (received_chars[rec_cnt++] == '\n') { - // execute the user command - run(received_chars); - - // reset the command buffer - received_chars[0] = 0; - rec_cnt=0; - } - } + run(*com_port, eol); } -void Commander::run(Stream& serial){ +void Commander::run(Stream& serial, char eol){ Stream* tmp = com_port; // save the serial instance - // use the new serial instance to output if not available the one linked in constructor - if(!tmp) com_port = &serial; + char eol_tmp = this->eol; + this->eol = eol; + com_port = &serial; // a string to hold incoming data while (serial.available()) { // get the new byte: - received_chars[rec_cnt] = (char)serial.read(); + int ch = serial.read(); + received_chars[rec_cnt++] = (char)ch; // end of user input - if (received_chars[rec_cnt++] == '\n') { + if(echo) + print((char)ch); + if (isSentinel(ch)) { // execute the user command run(received_chars); @@ -56,11 +50,15 @@ void Commander::run(Stream& serial){ } com_port = tmp; // reset the instance to the internal value + this->eol = eol_tmp; } void Commander::run(char* user_input){ // execute the user command char id = user_input[0]; + + + switch(id){ case CMD_SCAN: for(int i=0; i < call_count; i++){ @@ -71,7 +69,7 @@ void Commander::run(char* user_input){ } break; case CMD_VERBOSE: - if(user_input[1] != '\n') verbose = (VerboseMode)atoi(&user_input[1]); + if(user_input[1] != eol) verbose = (VerboseMode)atoi(&user_input[1]); printVerbose(F("Verb:")); switch (verbose){ case VerboseMode::nothing: @@ -84,7 +82,7 @@ void Commander::run(char* user_input){ } break; case CMD_DECIMAL: - if(user_input[1] != '\n') decimal_places = atoi(&user_input[1]); + if(user_input[1] != eol) decimal_places = atoi(&user_input[1]); printVerbose(F("Decimal:")); println(decimal_places); break; @@ -105,7 +103,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { char sub_cmd = user_command[1]; int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; // check if get command - bool GET = user_command[value_index] == '\n'; + bool GET = isSentinel(user_command[value_index]); // parse command values float value = atof(&user_command[value_index]); @@ -306,7 +304,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_SET: if(!GET) motor->monitor_variables = (uint8_t) 0; for(int i = 0; i < 7; i++){ - if(user_command[value_index+i] == '\n') break; + if(isSentinel(user_command[value_index+i])) break; if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); print( (user_command[value_index+i] - '0') ); } @@ -326,7 +324,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { void Commander::pid(PIDController* pid, char* user_cmd){ char cmd = user_cmd[0]; - bool GET = user_cmd[1] == '\n'; + bool GET = isSentinel(user_cmd[1]); float value = atof(&user_cmd[1]); switch (cmd){ @@ -363,7 +361,7 @@ void Commander::pid(PIDController* pid, char* user_cmd){ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ char cmd = user_cmd[0]; - bool GET = user_cmd[1] == '\n'; + bool GET = isSentinel(user_cmd[1]); float value = atof(&user_cmd[1]); switch (cmd){ @@ -379,11 +377,26 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ } void Commander::scalar(float* value, char* user_cmd){ - bool GET = user_cmd[0] == '\n'; + bool GET = isSentinel(user_cmd[0]); if(!GET) *value = atof(user_cmd); println(*value); } +bool Commander::isSentinel(char ch) +{ + if(ch == eol) + return true; + else if (ch == '\r') + { + if(verbose == VerboseMode::user_friendly) + { + print(F("Warning! \\r detected but is not configured as end of line sentinel, which is configured as ascii code '")); + print(int(eol)); + print("'\n"); + } + } + return false; +} void Commander::print(const int number){ if( !com_port || verbose == VerboseMode::nothing ) return; diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 0b07b707..6067e4d6 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -38,9 +38,11 @@ class Commander * 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) */ - Commander(Stream &serial); - Commander(); + Commander(Stream &serial, char eol = '\n', bool echo = false); + Commander(char eol = '\n', bool echo = false); /** * Function reading the serial port and firing callbacks that have been added to the commander @@ -61,9 +63,10 @@ class Commander * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes * - * @param reader - Stream to read user input + * @param reader - temporary stream to read user input + * @param eol - temporary end of line sentinel */ - void run(Stream &reader); + 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 @@ -91,7 +94,8 @@ class Commander // monitoring functions Stream* com_port = nullptr; //!< Serial terminal variable if provided - + char eol = '\n'; //!< end of line sentinel character + bool echo = false; //!< echo last typed character (for command line feedback) /** * * FOC motor (StepperMotor and BLDCMotor) command interface @@ -194,6 +198,7 @@ class Commander * @param message - number to be printed * @param newline - if needs lewline (1) otherwise (0) */ + void print(const float number); void print(const int number); void print(const char* message); @@ -206,6 +211,7 @@ class Commander void println(const char message); void printError(); + bool isSentinel(char ch); }; From 2abb7ed1c741acfb0fb87aa36fad721f7f5ecfd8 Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 10:46:28 -0600 Subject: [PATCH 26/45] Issue #46 : missing use of isSentinel --- src/communication/Commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index e9672325..c2413386 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -69,7 +69,7 @@ void Commander::run(char* user_input){ } break; case CMD_VERBOSE: - if(user_input[1] != eol) verbose = (VerboseMode)atoi(&user_input[1]); + if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]); printVerbose(F("Verb:")); switch (verbose){ case VerboseMode::nothing: @@ -82,7 +82,7 @@ void Commander::run(char* user_input){ } break; case CMD_DECIMAL: - if(user_input[1] != eol) decimal_places = atoi(&user_input[1]); + if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]); printVerbose(F("Decimal:")); println(decimal_places); break; From 5a9f0922ad621bf3ca9789d80a3962370ea4891f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:36:45 +0200 Subject: [PATCH 27/45] prevent buffer overrun when commands sent are too long --- src/communication/Commander.cpp | 4 ++++ src/communication/Commander.h | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index fae5165f..681424d4 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -32,6 +32,10 @@ void Commander::run(){ received_chars[0] = 0; rec_cnt=0; } + if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long + received_chars[0] = 0; + rec_cnt=0; + } } } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 0b07b707..a14c20b9 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -7,6 +7,10 @@ #include "../common/lowpass_filter.h" #include "commands.h" + +#define MAX_COMMAND_LENGTH 20 + + // Commander verbose display to the user type enum VerboseMode{ nothing = 0, // display nothing - good for monitoring @@ -174,7 +178,7 @@ class Commander int call_count = 0;//!< number callbacks that are subscribed // helping variable for serial communication reading - char received_chars[20] = {0}; //!< so far received user message - waiting for newline + 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 From ef7cf736ea983c2e946fbd42af998e06237f249b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:38:54 +0200 Subject: [PATCH 28/45] unfortunately, the string "EXTERN" is defined in rp2040 headers I renamed the enum member EXTERN to EXTERNAL. For consistency, I also renmaed INTERN. --- src/common/base_classes/Sensor.h | 6 +++--- src/sensors/Encoder.cpp | 4 ++-- src/sensors/HallSensor.cpp | 4 ++-- src/sensors/MagneticSensorAnalog.cpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index c7771a05..1a14e068 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -15,8 +15,8 @@ enum Direction{ * Pullup configuration structure */ enum Pullup{ - INTERN, //!< Use internal pullups - EXTERN //!< Use external pullups + INTERNAL, //!< Use internal pullups + EXTERNAL //!< Use external pullups }; /** @@ -43,4 +43,4 @@ class Sensor{ long velocity_calc_timestamp=0; //!< last velocity calculation timestamp }; -#endif \ No newline at end of file +#endif diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 5b196d57..803ea94a 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -32,7 +32,7 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ prev_timestamp_us = _micros(); // extern pullup as default - pullup = Pullup::EXTERN; + pullup = Pullup::EXTERNAL; // enable quadrature encoder by default quadrature = Quadrature::ON; } @@ -160,7 +160,7 @@ int Encoder::hasIndex(){ void Encoder::init(){ // Encoder - check if pullup needed for your encoder - if(pullup == Pullup::INTERN){ + if(pullup == Pullup::INTERNAL){ pinMode(pinA, INPUT_PULLUP); pinMode(pinB, INPUT_PULLUP); if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index e39fd514..3e32c448 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -17,7 +17,7 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ cpr = _pp * 6; // extern pullup as default - pullup = Pullup::EXTERN; + pullup = Pullup::EXTERNAL; } // HallSensor interrupt callback functions @@ -119,7 +119,7 @@ void HallSensor::init(){ electric_rotations = 0; // HallSensor - check if pullup needed for your HallSensor - if(pullup == Pullup::INTERN){ + if(pullup == Pullup::INTERNAL){ pinMode(pinA, INPUT_PULLUP); pinMode(pinB, INPUT_PULLUP); pinMode(pinC, INPUT_PULLUP); diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 0216392a..52492adf 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -13,7 +13,7 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - if(pullup == Pullup::INTERN){ + if(pullup == Pullup::INTERNAL){ pinMode(pinAnalog, INPUT_PULLUP); }else{ pinMode(pinAnalog, INPUT); From 1fa5dd7b4f53c94647b0e1d8ee33ba84be8b2a47 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:40:17 +0200 Subject: [PATCH 29/45] the constants SDA and SCL are not defined in rp2040. The solution is to define them based on the equivalent rp2040 constants --- src/sensors/MagneticSensorI2C.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 6ff2c761..9966d6eb 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -16,6 +16,12 @@ struct MagneticSensorI2CConfig_s { // some predefined structures extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; +#if defined(TARGET_RP2040) +#define SDA I2C_SDA +#define SCL I2C_SCL +#endif + + class MagneticSensorI2C: public Sensor{ public: /** From 8da3507121149c833cbc1241cca0e0cbbff2e119 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:41:58 +0200 Subject: [PATCH 30/45] unfortunately this code does not work with rp2040 until I find a way to make it compatible, it is disabled when compling for rp2040. Problem is that SPIClass and SPISettings are not defined, SPI seems to work somewhat differently in rp2040 --- src/sensors/MagneticSensorSPI.cpp | 5 +++++ src/sensors/MagneticSensorSPI.h | 3 +++ 2 files changed, 8 insertions(+) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 56833b7f..b3d82dee 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -1,3 +1,5 @@ +#ifndef TARGET_RP2040 + #include "MagneticSensorSPI.h" /** Typical configuration for the 14bit AMS AS5147 magnetic sensor over SPI interface */ @@ -212,3 +214,6 @@ word MagneticSensorSPI::read(word angle_register){ void MagneticSensorSPI::close(){ spi->end(); } + + +#endif diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 764053df..77fcde4d 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -1,6 +1,8 @@ #ifndef MAGNETICSENSORSPI_LIB_H #define MAGNETICSENSORSPI_LIB_H +#ifndef TARGET_RP2040 + #include "Arduino.h" #include #include "../common/base_classes/Sensor.h" @@ -91,3 +93,4 @@ class MagneticSensorSPI: public Sensor{ #endif +#endif From 3fc05d2edeb73d918a202ac226c269f98988bbba Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:42:28 +0200 Subject: [PATCH 31/45] rp2040 (aka Raspberry Pi Pico) support --- src/drivers/hardware_specific/generic_mcu.cpp | 2 + src/drivers/hardware_specific/rp2040_mcu.cpp | 163 ++++++++++++++++++ 2 files changed, 165 insertions(+) create mode 100644 src/drivers/hardware_specific/rp2040_mcu.cpp diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index fd64e6af..4a2360d9 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -18,6 +18,8 @@ #elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 +#elif defined(TARGET_RP2040) + #else // function setting the high pwm frequency to the supplied pins diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp new file mode 100644 index 00000000..097b7a6d --- /dev/null +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -0,0 +1,163 @@ + +/** + * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. + */ +#if defined(TARGET_RP2040) + +#define SIMPLEFOC_DEBUG_RP2040 + + +#ifdef SIMPLEFOC_DEBUG_RP2040 + +#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL +#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial +#endif + +#endif + +#include "Arduino.h" + + + + +// until I can figure out if this can be quickly read from some register, keep it here. +// it also serves as a marker for what slices are already used. +uint16_t wrapvalues[NUM_PWM_SLICES]; + + +// TODO add checks which channels are already used... + +void setupPWM(int pin, long pwm_frequency, bool invert = false) { + gpio_set_function(pin, GPIO_FUNC_PWM); + uint slice = pwm_gpio_to_slice_num(pin); + uint chan = pwm_gpio_to_channel(pin); + pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get + pwm_set_phase_correct(slice, true); + uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; + if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 + if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 +#ifdef SIMPLEFOC_DEBUG_RP2040 + SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin "); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice "); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel "); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency "); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency); + SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value "); + SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue); +#endif + pwm_set_wrap(slice, wrapvalue); + wrapvalues[slice] = wrapvalue; + if (invert) { + if (chan==0) + hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS); + else + hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS); + } + pwm_set_chan_level(slice, chan, 0); // switch off initially +} + + +void syncSlices() { + for (int i=0;i1.0) ret = 1.0; + return ret; +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { + writeDutyCycle(dc_a, pinA_h); + writeDutyCycle(swDti(dc_a, dead_zone), pinA_l); + writeDutyCycle(dc_b, pinB_h); + writeDutyCycle(swDti(dc_b,dead_zone), pinB_l); + writeDutyCycle(dc_c, pinC_h); + writeDutyCycle(swDti(dc_c,dead_zone), pinC_l); +} + +#endif From 9220eaddaed7a3544fd8c3bea4d678e7715eb2d6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:16:22 +0200 Subject: [PATCH 32/45] division by zero issue fix #76 + defines for default pwm freq --- src/drivers/hardware_specific/esp32_mcu.cpp | 26 +++++++++++--------- src/drivers/hardware_specific/teensy_mcu.cpp | 15 ++++++----- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index b9c23d9c..0bc2d863 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -20,6 +20,9 @@ #define _PWM_RES_MIN 1500 // max resolution #define _PWM_RES_MAX 3000 +// pwm frequency +#define _PWM_FREQUENCY 25000 // default +#define _PWM_FREQUENCY_MAX 50000 // mqx // structure containing motor slot configuration // this library supports up to 4 motors @@ -102,9 +105,10 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_config_t pwm_config; pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 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 PWM0A & PWM0B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &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 @@ -149,6 +153,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[1].period.upmethod = 0; mcpwm_num->timer[2].period.upmethod = 0; _delay(1); + // _delay(1); //restart the timers mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); @@ -171,9 +176,8 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // - hardware speciffic // 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 = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_2pwm_motor_slots_t m_slot = {}; @@ -217,9 +221,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - hardware speciffic // 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 = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + 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 bldc_3pwm_motor_slots_t m_slot = {}; @@ -262,9 +265,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - 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 = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 50kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters @@ -367,7 +369,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in 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, 40000); // constrain to 40kHz max - 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 diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index e95f2066..8ec19643 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -2,6 +2,9 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#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); @@ -13,8 +16,8 @@ void _setHighFrequency(const long freq, const int pin){ // - 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 = 50000; // 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 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); } @@ -23,8 +26,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // - 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 = 50000; // 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 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); @@ -34,8 +37,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - 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 = 50000; // 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 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); From e3c91e13be8f566d56a476bfd01f6e4282c1e1a6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:31:21 +0200 Subject: [PATCH 33/45] reduced \r error string + target setting separated + unknowm cmd error --- src/communication/Commander.cpp | 34 +++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index ddd67b83..72d5e90b 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -54,15 +54,12 @@ void Commander::run(Stream& serial, char eol){ } com_port = tmp; // reset the instance to the internal value - this->eol = eol_tmp; + this->eol = eol_tmp; } void Commander::run(char* user_input){ // execute the user command char id = user_input[0]; - - - switch(id){ case CMD_SCAN: for(int i=0; i < call_count; i++){ @@ -102,14 +99,25 @@ void Commander::run(char* user_input){ } void Commander::motor(FOCMotor* motor, char* user_command) { + + // if target setting + if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){ + printVerbose(F("Target: ")); + motor->target = atof(user_command); + println(motor->target); + return; + } + // parse command letter char cmd = user_command[0]; char sub_cmd = user_command[1]; + // check if there is a subcommand or not int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; // check if get command bool GET = isSentinel(user_command[value_index]); // parse command values - float value = atof(&user_command[value_index]); + float value = atof(&user_command[value_index]); + // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ @@ -181,7 +189,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; default: // change control type - if(!GET && value >= 0 && (int)value < 5)// if set command + if(!GET && value >= 0 && (int)value < 5) // if set command motor->controller = (MotionControlType)value; switch(motor->controller){ case MotionControlType::torque: @@ -367,10 +375,9 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; } break; - default: // target change - printVerbose(F("Target: ")); - motor->target = atof(user_command); - println(motor->target); + default: // unknown cmd + printVerbose(F("unknown cmd ")); + printError(); } } @@ -440,12 +447,7 @@ bool Commander::isSentinel(char ch) return true; else if (ch == '\r') { - if(verbose == VerboseMode::user_friendly) - { - print(F("Warning! \\r detected but is not configured as end of line sentinel, which is configured as ascii code '")); - print(int(eol)); - print("'\n"); - } + printVerbose(F("Warn: \\r detected! \n")); } return false; } From ffdc4b15e9d5a33ee62f3e5a0ac0e8dd83a2d0bd Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:33:12 +0200 Subject: [PATCH 34/45] dead zone as5600 register fix - set to 0x0C --- .../bluepill_position_control/bluepill_position_control.ino | 2 +- .../alignment_and_cogging_test/alignment_and_cogging_test.ino | 2 +- .../find_pole_pairs_number/find_pole_pairs_number.ino | 2 +- .../find_sensor_offset_and_direction.ino | 2 +- src/sensors/MagneticSensorI2C.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) 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 54608e8a..0adf4c17 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 @@ -17,7 +17,7 @@ MagneticSensorSPI sensor = MagneticSensorSPI(PA4, 14, 0x3FFF); // make sure to use the pull-ups!! // SDA PB7 // SCL PB6 -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); // Motor instance BLDCMotor motor = BLDCMotor(11); 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 8292ecf4..708e2d9e 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 @@ -5,7 +5,7 @@ BLDCMotor motor = BLDCMotor(11); BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); -MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); /** 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 4f6e83cc..cc4bd4d0 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 @@ -27,7 +27,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - I2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index a54fcea4..3ad87cca 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -15,7 +15,7 @@ // magnetic sensor instance - SPI //MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - I2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); // magnetic sensor instance - analog output MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 531a06f9..c9029f6e 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -4,7 +4,7 @@ MagneticSensorI2CConfig_s AS5600_I2C = { .chip_address = 0x36, .bit_resolution = 12, - .angle_register = 0x0E, + .angle_register = 0x0C, .data_start_bit = 11 }; From 39d2cefb9266c9dda8176ed79063548a47bb0831 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:35:20 +0200 Subject: [PATCH 35/45] removed old fix in drivers for issue #76 --- src/drivers/StepperDriver4PWM.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index d3678f00..f752ea6e 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -39,8 +39,6 @@ void StepperDriver4PWM::disable() // init hardware pins int StepperDriver4PWM::init() { - // a bit of separation - _delay(1000); // PWM pins pinMode(pwm1A, OUTPUT); From b893f1139ff7a9b3f17051d847618f7fb569928e Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:36:20 +0200 Subject: [PATCH 36/45] removed old fix in drivers for issue #76 2 --- src/drivers/BLDCDriver3PWM.cpp | 3 --- src/drivers/BLDCDriver6PWM.cpp | 4 +--- src/drivers/StepperDriver2PWM.cpp | 3 --- 3 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 87eb7f5b..11215863 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -41,9 +41,6 @@ void BLDCDriver3PWM::disable() // init hardware pins int BLDCDriver3PWM::init() { - // a bit of separation - _delay(1000); - // PWM pins pinMode(pwmA, OUTPUT); pinMode(pwmB, OUTPUT); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 5dd99eb2..d51eec0a 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -41,9 +41,7 @@ void BLDCDriver6PWM::disable() // init hardware pins int BLDCDriver6PWM::init() { - // a bit of separation - _delay(1000); - + // PWM pins pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 7ca7cc31..09dd3e16 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -61,9 +61,6 @@ void StepperDriver2PWM::disable() // init hardware pins int StepperDriver2PWM::init() { - // a bit of separation - _delay(1000); - // PWM pins pinMode(pwm1, OUTPUT); pinMode(pwm2, OUTPUT); From b5dccfb0fda9f035099496d474e8e7dda7c02e9b Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:42:23 +0200 Subject: [PATCH 37/45] readme prepared for v2.1.1 --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 04b33478..2d1aa0fa 100644 --- a/README.md +++ b/README.md @@ -24,8 +24,13 @@ Therefore this is an attempt to: > - added initial support for `MagneticSensorPWM` > - 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 ## Arduino *SimpleFOClibrary* v2.1 From bf45b6a18007b85e22ee124e95ce0d88a9c981c3 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:59:22 +0200 Subject: [PATCH 38/45] FIX phase resitence not changing voltage_limit --- src/communication/Commander.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 72d5e90b..8e1cb051 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -159,10 +159,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) { printVerbose(F("curr: ")); if(!GET){ motor->current_limit = value; - // if phase resistance is set, change the voltage limit as well. - if(_isset(motor->phase_resistance)) motor->voltage_limit = value*motor->phase_resistance; // if phase resistance specified or the current control is on set the current limit to the velocity PID - if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value; + if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value; } println(motor->current_limit); break; From 3ec3038fada0b11987f362ecacde7beaab6d8d54 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 13:13:50 +0200 Subject: [PATCH 39/45] examples for 6mOhm resistor --- .../double_full_control_example/double_full_control_example.ino | 2 ++ .../single_full_control_example/single_full_control_example.ino | 1 + 2 files changed, 3 insertions(+) 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 17b98ce5..757ae0f1 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 @@ -23,9 +23,11 @@ void doB2(){encoder2.handleB();} // inline current sensor instance +// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2); // inline current sensor instance +// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); // commander communication instance 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 70444520..c0f6a10b 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 @@ -12,6 +12,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} // inline current sensor instance +// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance From d1b41d01767a88ecdf03af13b3fdb47d28193726 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 13:53:06 +0200 Subject: [PATCH 40/45] v2.1.1 update readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2d1aa0fa..eaf534e0 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) -##### Next release will be: SimpleFOClibrary v2.1.1 +##### Release notes be: SimpleFOClibrary v2.1.1 > - bugfixes commander > - bugfix `voltage_limit` when provided `phase_resistance` > - bugfix `hall_sensor` examples From a3741bf35e316900f4823734d67fca84a03b597f Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 14:10:42 +0200 Subject: [PATCH 41/45] readme html problem resolution --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index eaf534e0..33c8b2eb 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.1 +##### Release notes be: SimpleFOClibrary v2.1.1 > - bugfixes commander > - bugfix `voltage_limit` when provided `phase_resistance` > - bugfix `hall_sensor` examples From 5fcfbfe9cb569af54847b632a2052db2e4a5c5b4 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 16:42:38 +0200 Subject: [PATCH 42/45] FEAT pwm sensor can be blocking or nonblocking --- src/sensors/MagneticSensorPWM.cpp | 19 +++++++++++++------ src/sensors/MagneticSensorPWM.h | 4 ++++ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 4e9f944e..da4ebf89 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -14,15 +14,19 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - pinMode(pinPWM, INPUT); + // define if the sensor uses interrupts + is_interrupt_based = false; - // init last call + // define as not set last_call_us = _micros(); } void MagneticSensorPWM::init(){ + // initial hardware + pinMode(pinPWM, INPUT); + // velocity calculation init angle_prev = 0; velocity_calc_timestamp = _micros(); @@ -66,10 +70,9 @@ float MagneticSensorPWM::getVelocity(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 - pulse_length_us = pulseIn(pinPWM, HIGH); -#endif - + if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way + pulse_length_us = pulseIn(pinPWM, HIGH); + } return pulse_length_us; } @@ -83,11 +86,15 @@ void MagneticSensorPWM::handlePWM() { // save the currrent timestamp for the next call last_call_us = now_us; + is_interrupt_based = true; // set the flag to true } // function enabling hardware interrupts of the for the callback provided // if callback is not provided then the interrupt is not enabled 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); diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 46053ac7..32d6daee 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -38,6 +38,10 @@ class MagneticSensorPWM: public Sensor{ int min_raw_count; int max_raw_count; int cpr; + + // flag saying if the readings are interrupt based or not + bool is_interrupt_based; + int read(); /** From b1d26d2311c610c43d98f2bca48b9ba963ff28d6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 16:44:10 +0200 Subject: [PATCH 43/45] examples for nonblocking --- .../find_raw_min_max/find_raw_min_max.ino | 1 + .../magnetic_sensor_pwm/magnetic_sensor_pwm.ino | 1 + .../magnetic_sensor_pwm_software_interrupt.ino | 1 + 3 files changed, 3 insertions(+) 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_example/find_raw_min_max/find_raw_min_max.ino index 27816eec..a88b7186 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_example/find_raw_min_max/find_raw_min_max.ino @@ -16,6 +16,7 @@ void setup() { // initialise magnetic sensor hardware sensor.init(); + // comment out to use sensor in blocking (non-interrupt) way sensor.enableInterrupt(doPWM); Serial.println("Sensor ready"); 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_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino index 8d0bd54f..15a1f557 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_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino @@ -18,6 +18,7 @@ void setup() { // initialise magnetic sensor hardware sensor.init(); + // comment out to use sensor in blocking (non-interrupt) way sensor.enableInterrupt(doPWM); Serial.println("Sensor ready"); 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_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino index 5b7665a0..6b19a9fa 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_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -24,6 +24,7 @@ void setup() { // initialise magnetic sensor hardware sensor.init(); + // comment out to use sensor in blocking (non-interrupt) way PciManager.registerListener(&listenerPWM); Serial.println("Sensor ready"); From 3fde38d5af6e94b23323e10009acb240ffb44893 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 19:22:33 +0200 Subject: [PATCH 44/45] support for RPI Pico to readme + Pullup naming update --- README.md | 3 +++ keywords.txt | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 33c8b2eb..56d2a57d 100644 --- a/README.md +++ b/README.md @@ -22,6 +22,8 @@ Therefore this is an attempt to: > - 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` @@ -31,6 +33,7 @@ Therefore this is an attempt to: > - 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::EXTERNAL` and `Pullup::INTERNAL` ## Arduino *SimpleFOClibrary* v2.1 diff --git a/keywords.txt b/keywords.txt index 971a3ffb..23699b3b 100644 --- a/keywords.txt +++ b/keywords.txt @@ -164,8 +164,8 @@ pinB KEYWORD2 pinC KEYWORD2 index_pin KEYWORD2 -INTERN KEYWORD2 -EXTERN KEYWORD2 +INTERNAL KEYWORD2 +EXTERNAL KEYWORD2 DISABLE KEYWORD2 ENABLE KEYWORD2 SpaceVectorPWM KEYWORD2 From a4f99990d7a120a2b0d3dfaaca9576b4f0d5656b Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 19:39:01 +0200 Subject: [PATCH 45/45] Annoying pullup struct rename from EXTERN/INTERN to USE_EXTERN/USE_INTERN --- README.md | 2 +- .../sensor_test/encoder/encoder_example/encoder_example.ino | 2 +- .../encoder_software_interrupts_example.ino | 2 +- .../hall_sensors/hall_sensor_example/hall_sensor_example.ino | 2 +- .../hall_sensors_software_interrupt_example.ino | 2 +- keywords.txt | 4 ++-- src/common/base_classes/Sensor.h | 4 ++-- src/sensors/Encoder.cpp | 4 ++-- src/sensors/HallSensor.cpp | 4 ++-- src/sensors/MagneticSensorAnalog.cpp | 2 +- 10 files changed, 14 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 56d2a57d..b9042658 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ Therefore this is an attempt to: > - 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::EXTERNAL` and `Pullup::INTERNAL` +> - renamed `Pullup::EXTERN` and `Pullup::INTERN` to `Pullup::USE_EXTERN` and `Pullup::USE_INTERN` ## Arduino *SimpleFOClibrary* v2.1 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 0fa87522..66aca6d8 100644 --- a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino @@ -21,7 +21,7 @@ void setup() { encoder.quadrature = Quadrature::ON; // check if you need internal pullups - encoder.pullup = Pullup::EXTERN; + encoder.pullup = Pullup::USE_EXTERN; // initialise encoder hardware encoder.init(); 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 7c38bb59..a979d9a0 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 @@ -32,7 +32,7 @@ void setup() { encoder.quadrature = Quadrature::ON; // check if you need internal pullups - encoder.pullup = Pullup::EXTERN; + encoder.pullup = Pullup::USE_EXTERN; // initialise encoder hardware encoder.init(); 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 965aec7a..6c19a2e9 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 @@ -25,7 +25,7 @@ void setup() { Serial.begin(115200); // check if you need internal pullups - sensor.pullup = Pullup::EXTERN; + sensor.pullup = Pullup::USE_EXTERN; // initialise encoder hardware sensor.init(); 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 cac8dc00..523af03f 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 @@ -34,7 +34,7 @@ void setup() { Serial.begin(115200); // check if you need internal pullups - sensor.pullup = Pullup::EXTERN; + sensor.pullup = Pullup::USE_EXTERN; // initialise encoder hardware sensor.init(); diff --git a/keywords.txt b/keywords.txt index 23699b3b..77152a36 100644 --- a/keywords.txt +++ b/keywords.txt @@ -164,8 +164,8 @@ pinB KEYWORD2 pinC KEYWORD2 index_pin KEYWORD2 -INTERNAL KEYWORD2 -EXTERNAL KEYWORD2 +USE_INTERN KEYWORD2 +USE_EXTERN KEYWORD2 DISABLE KEYWORD2 ENABLE KEYWORD2 SpaceVectorPWM KEYWORD2 diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 1a14e068..3b219401 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -15,8 +15,8 @@ enum Direction{ * Pullup configuration structure */ enum Pullup{ - INTERNAL, //!< Use internal pullups - EXTERNAL //!< Use external pullups + USE_INTERN, //!< Use internal pullups + USE_EXTERN //!< Use external pullups }; /** diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 803ea94a..001330ba 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -32,7 +32,7 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ prev_timestamp_us = _micros(); // extern pullup as default - pullup = Pullup::EXTERNAL; + pullup = Pullup::USE_EXTERN; // enable quadrature encoder by default quadrature = Quadrature::ON; } @@ -160,7 +160,7 @@ int Encoder::hasIndex(){ void Encoder::init(){ // Encoder - check if pullup needed for your encoder - if(pullup == Pullup::INTERNAL){ + if(pullup == Pullup::USE_INTERN){ pinMode(pinA, INPUT_PULLUP); pinMode(pinB, INPUT_PULLUP); if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 3e32c448..2ceb22cf 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -17,7 +17,7 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ cpr = _pp * 6; // extern pullup as default - pullup = Pullup::EXTERNAL; + pullup = Pullup::USE_EXTERN; } // HallSensor interrupt callback functions @@ -119,7 +119,7 @@ void HallSensor::init(){ electric_rotations = 0; // HallSensor - check if pullup needed for your HallSensor - if(pullup == Pullup::INTERNAL){ + if(pullup == Pullup::USE_INTERN){ pinMode(pinA, INPUT_PULLUP); pinMode(pinB, INPUT_PULLUP); pinMode(pinC, INPUT_PULLUP); diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 52492adf..35f6d73c 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -13,7 +13,7 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - if(pullup == Pullup::INTERNAL){ + if(pullup == Pullup::USE_INTERN){ pinMode(pinAnalog, INPUT_PULLUP); }else{ pinMode(pinAnalog, INPUT);