From 134474ae91fb79867440f4fd1dd47e8230a8f546 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 12 Oct 2020 08:27:43 +0100 Subject: [PATCH 001/749] adding support for two i2c buses --- .../angle_control/angle_control.ino | 4 +- .../voltage_control/voltage_control.ino | 6 +-- .../velocity_control/velocity_control.ino | 9 ++-- .../full_control_serial.ino | 8 ++-- .../magnetic_sensor_i2c_dual_bus_example.ino | 46 +++++++++++++++++++ .../magnetic_sensor_i2c_example.ino | 8 +++- .../magnetic_sensor_spi_example.ino | 12 +++-- src/MagneticSensorI2C.cpp | 42 +++++------------ src/MagneticSensorI2C.h | 16 ++----- 9 files changed, 89 insertions(+), 62 deletions(-) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index bb110f65..af3fe719 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -10,9 +10,9 @@ #include // magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); // magnetic sensor instance - MagneticSensorI2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino index d9a2d2be..dcb9c450 100644 --- a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -9,9 +9,9 @@ #include // magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); -// magnetic sensor instance - I2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +// magnetic sensor instance - MagneticSensorI2C +//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 3caf282a..b907afd3 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -12,11 +12,10 @@ */ #include -// magnetic sensor instance -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); -// magnetic sensor instance -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); -// magnetic sensor instance - analog output +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +// magnetic sensor instance - MagneticSensorI2C +//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); // BLDC motor instance diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 73434cfb..375ea198 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -38,10 +38,10 @@ */ #include -// SPI magnetic sensor instance -// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); -// I2C magnetic sensor instance -MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +// magnetic sensor instance - MagneticSensorI2C +//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino new file mode 100644 index 00000000..27cce1ca --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino @@ -0,0 +1,46 @@ +#include + +/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus + * This example shows how a second i2c bus can be used to communicate with a second sensor. + */ + +MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); +MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); + +#if defined(_STM32_DEF_) // if stm chips + // example of stm32 defining 2nd bus + TwoWire Wire1(PB11, PB10); + +#elif defined(ESP_H) // if esp32 + // esp32 defines a Wire1 but doesn't define pins! + // nothing to do here for esp32! (See below) +#else + // Wire constructors vary - you'll have to check what works for your chip + TwoWire Wire1(SDA1, SCL1); +#endif + +void setup() { + + Serial.begin(115200); + _delay(750); + + Wire.setClock(400000); + Wire1.setClock(400000); + + #if defined(ESP_H) // if esp32 + // Normally SimpeFOC will call begin for i2c but with esp32 begin() is the only way to set pins! + // It seems safe to call begin multiple times + Wire1.begin(19,23,400000); + #endif + + sensor0.init(); + sensor1.init(&Wire1); +} + +void loop() { + _delay(200); + Serial.print(sensor0.getAngle()); + Serial.print(" - "); + Serial.print(sensor1.getAngle()); + Serial.println(); +} diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino index ad67eb03..30afc90c 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino @@ -9,15 +9,19 @@ // make sure to read the chip address and the chip angle register msb value from the datasheet // also in most cases you will need external pull-ups on SDA and SCL lines!!!!! // -// For AS5058B use MagneticSensorI2C(0x40, 14, 0xFE, 8) +// For AS5058B +// MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8); + // Example of AS5600 configuration -MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); void setup() { // monitoring port Serial.begin(115200); + // configure i2C + Wire.setClock(400000); // initialise magnetic sensor hardware sensor.init(); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino index 828162dc..319c3741 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino @@ -1,10 +1,12 @@ #include -// MagneticSensorSPI(int cs, float _cpr, int _angle_register) -// cs - SPI chip select pin -// bit_resolution - sensor resolution -// angle_register - (optional) angle read register - default 0x3FFF -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); +// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs) +// config - SPI config +// cs - SPI chip select pin +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +// alternative constructor (chipselsect, bit_resolution, angle_read_register, ) +// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); void setup() { // monitoring port diff --git a/src/MagneticSensorI2C.cpp b/src/MagneticSensorI2C.cpp index a02b762b..5df6e7eb 100644 --- a/src/MagneticSensorI2C.cpp +++ b/src/MagneticSensorI2C.cpp @@ -3,7 +3,6 @@ /** Typical configuration for the 12bit AMS AS5600 magnetic sensor over I2C interface */ MagneticSensorI2CConfig_s AS5600_I2C = { .chip_address = 0x36, - .clock_speed = 1000000, .bit_resolution = 12, .angle_register = 0x0E, .data_start_bit = 11 @@ -12,7 +11,6 @@ MagneticSensorI2CConfig_s AS5600_I2C = { /** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */ MagneticSensorI2CConfig_s AS5048_I2C = { .chip_address = 0x40, // highly configurable. if A1 and A2 are held low, this is probable value - .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0xFE, .data_start_bit = 15 @@ -41,10 +39,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // extraction masks lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); - clock_speed = 400000; - sda_pin = SDA; - scl_pin = SCL; - + wire = &Wire; } MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ @@ -60,29 +55,16 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ // extraction masks lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 ); - clock_speed = 400000; - sda_pin = SDA; - scl_pin = SCL; - + wire = &Wire; } -void MagneticSensorI2C::init(){ +void MagneticSensorI2C::init(TwoWire* _wire){ + + wire = _wire; -#if defined(_STM32_DEF_) // if stm chips // I2C communication begin - Wire.begin(); - Wire.setClock(clock_speed); - Wire.setSCL(scl_pin); - Wire.setSDA(sda_pin); -#elif defined(ESP_H) // if esp32 - //I2C communication begin - Wire.begin(sda_pin, scl_pin, clock_speed); -#else - // I2C communication begin - Wire.begin(); - Wire.setClock(clock_speed); -#endif - + wire->begin(); + // velocity calculation init angle_prev = 0; velocity_calc_timestamp = _micros(); @@ -187,14 +169,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { byte readArray[2]; uint16_t readValue = 0; // notify the device that is aboout to be read - Wire.beginTransmission(chip_address); - Wire.write(angle_reg_msb); - Wire.endTransmission(false); + wire->beginTransmission(chip_address); + wire->write(angle_reg_msb); + wire->endTransmission(false); // read the data msb and lsb - Wire.requestFrom(chip_address, (uint8_t)2); + wire->requestFrom(chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { - readArray[i] = Wire.read(); + readArray[i] = wire->read(); } // depending on the sensor architecture there are different combinations of diff --git a/src/MagneticSensorI2C.h b/src/MagneticSensorI2C.h index 88d74d35..ff496136 100644 --- a/src/MagneticSensorI2C.h +++ b/src/MagneticSensorI2C.h @@ -9,7 +9,6 @@ struct MagneticSensorI2CConfig_s { int chip_address; - long clock_speed; int bit_resolution; int angle_register; int data_start_bit; @@ -37,7 +36,7 @@ class MagneticSensorI2C: public Sensor{ static MagneticSensorI2C AS5600(); /** sensor initialise pins */ - void init(); + void init(TwoWire* _wire = &Wire); // implementation of abstract functions of the Sensor class /** get current angle (rad) */ @@ -60,15 +59,6 @@ class MagneticSensorI2C: public Sensor{ int needsAbsoluteZeroSearch() override; - /* the speed of the i2c clock signal */ - long clock_speed; - - /* the pin used for i2c data */ - int sda_pin; - - /* the pin used for i2c clock */ - int scl_pin; - private: float cpr; //!< Maximum range of the magnetic sensor uint16_t lsb_used; //!< Number of bits used in LSB register @@ -98,6 +88,10 @@ class MagneticSensorI2C: public Sensor{ float angle_prev; //!< angle in previous velocity calculation step long velocity_calc_timestamp; //!< last velocity calculation timestamp + /* the two wire instance for this sensor */ + TwoWire* wire; + + }; From dc2d2560a9acb8a7cc2be2d58fcbf3b9a082f26b Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Thu, 15 Oct 2020 20:42:00 +0100 Subject: [PATCH 002/749] adding experimental i2c sda lock clearing --- src/MagneticSensorI2C.cpp | 43 +++++++++++++++++++++++++++++++++++++++ src/MagneticSensorI2C.h | 3 +++ 2 files changed, 46 insertions(+) diff --git a/src/MagneticSensorI2C.cpp b/src/MagneticSensorI2C.cpp index 5df6e7eb..accce08a 100644 --- a/src/MagneticSensorI2C.cpp +++ b/src/MagneticSensorI2C.cpp @@ -187,3 +187,46 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); return readValue; } + +/* +* Checks whether other devices have locked the bus. Can clear SDA locks. +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* e.g some stm32 boards with AS5600 chips +* Takes the sda_pin and scl_pin +* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW +*/ +int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { + + pinMode(scl_pin, INPUT_PULLUP); + pinMode(sda_pin, INPUT_PULLUP); + delay(250); + + if (digitalRead(scl_pin) == LOW) { + // Someone else has claimed master!"); + return 1; + } + + if(digitalRead(sda_pin) == LOW) { + // slave is communicating and awaiting clocks, we are blocked + pinMode(scl_pin, OUTPUT); + for (byte i = 0; i < 16; i++) { + // toggle clock for 2 bytes of data + digitalWrite(scl_pin, LOW); + delayMicroseconds(20); + digitalWrite(scl_pin, HIGH); + delayMicroseconds(20); + } + pinMode(sda_pin, INPUT); + delayMicroseconds(20); + if (digitalRead(sda_pin) == LOW) { + // SDA still blocked + return 2; + } + _delay(1000); + } + // SDA is clear (HIGH) + pinMode(sda_pin, INPUT); + pinMode(scl_pin, INPUT); + + return 0; +} diff --git a/src/MagneticSensorI2C.h b/src/MagneticSensorI2C.h index ff496136..3d031124 100644 --- a/src/MagneticSensorI2C.h +++ b/src/MagneticSensorI2C.h @@ -59,6 +59,9 @@ class MagneticSensorI2C: public Sensor{ int needsAbsoluteZeroSearch() override; + /** experimental function to check and fix SDA locked LOW issues */ + int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + private: float cpr; //!< Maximum range of the magnetic sensor uint16_t lsb_used; //!< Number of bits used in LSB register From a8fee49d7145317d6fc719d23e6fccc6bb2907e4 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Sun, 18 Oct 2020 21:59:33 +0100 Subject: [PATCH 003/749] Initial support for secondary SPI bus --- src/MagneticSensorSPI.cpp | 23 +++++++++++++---------- src/MagneticSensorSPI.h | 3 ++- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/MagneticSensorSPI.cpp b/src/MagneticSensorSPI.cpp index 1f63bc11..3bbd1946 100644 --- a/src/MagneticSensorSPI.cpp +++ b/src/MagneticSensorSPI.cpp @@ -59,7 +59,10 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ data_start_bit = config.data_start_bit; // for backwards compatibilty } -void MagneticSensorSPI::init(){ +void MagneticSensorSPI::init(SPIClass* _spi){ + + spi = _spi; + // 1MHz clock (AMS should be able to accept up to 10MHz) settings = SPISettings(clock_speed, MSBFIRST, spi_mode); @@ -67,11 +70,11 @@ void MagneticSensorSPI::init(){ pinMode(chip_select_pin, OUTPUT); //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices - SPI.begin(); + spi->begin(); #ifndef ESP_H // if not ESP32 board - SPI.setBitOrder(MSBFIRST); // Set the SPI_1 bit order - SPI.setDataMode(spi_mode) ; - SPI.setClockDivider(SPI_CLOCK_DIV8); + spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order + spi->setDataMode(spi_mode) ; + spi->setClockDivider(SPI_CLOCK_DIV8); #endif digitalWrite(chip_select_pin, HIGH); @@ -203,13 +206,13 @@ word MagneticSensorSPI::read(word angle_register){ #if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction - SPI.beginTransaction(settings); + spi->beginTransaction(settings); #endif //Send the command digitalWrite(chip_select_pin, LOW); digitalWrite(chip_select_pin, LOW); - SPI.transfer16(command); + spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); digitalWrite(chip_select_pin,HIGH); @@ -222,13 +225,13 @@ word MagneticSensorSPI::read(word angle_register){ //Now read the response digitalWrite(chip_select_pin, LOW); digitalWrite(chip_select_pin, LOW); - word register_value = SPI.transfer16(0x00); + word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); digitalWrite(chip_select_pin,HIGH); #if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction - SPI.endTransaction(); + spi->endTransaction(); #endif register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word @@ -243,5 +246,5 @@ word MagneticSensorSPI::read(word angle_register){ * SPI has an internal SPI-device counter, for each init()-call the close() function must be called exactly 1 time */ void MagneticSensorSPI::close(){ - SPI.end(); + spi->end(); } diff --git a/src/MagneticSensorSPI.h b/src/MagneticSensorSPI.h index 8e884cf4..4186ac1f 100644 --- a/src/MagneticSensorSPI.h +++ b/src/MagneticSensorSPI.h @@ -38,7 +38,7 @@ class MagneticSensorSPI: public Sensor{ MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs); /** sensor initialise pins */ - void init(); + void init(SPIClass* _spi = &SPI); // implementation of abstract functions of the Sensor class /** get current angle (rad) */ @@ -103,6 +103,7 @@ class MagneticSensorSPI: public Sensor{ int command_rw_bit; //!< the bit where read/write flag is stored in command int data_start_bit; //!< the the position of first bit + SPIClass* spi; }; From 8cab7cd709016dbe4e0a54f1c4f96bd9564c7e07 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Sun, 18 Oct 2020 22:08:07 +0100 Subject: [PATCH 004/749] and the example --- .../magnetic_sensor_spi_alt_example.ino | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino new file mode 100644 index 00000000..249837e6 --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino @@ -0,0 +1,27 @@ +#include + +// MagneticSensorSPI(int cs, float _cpr, int _angle_register) +// config - SPI config +// cs - SPI chip select pin +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, PA15); + +// these are valid pins (mosi, miso, sclk) for 2nd SPI bus on storm32 board (stm32f107rc) +SPIClass SPI_2(PB15, PB14, PB13); + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + sensor.init(&SPI_2); + + 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()); +} From 7b32eca43a36a257411bf879fb4a0eb5f39838ab Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Thu, 22 Oct 2020 23:16:23 +0100 Subject: [PATCH 005/749] fix spi datamask (word/int bug) --- src/MagneticSensorSPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MagneticSensorSPI.cpp b/src/MagneticSensorSPI.cpp index 1f63bc11..71acddb8 100644 --- a/src/MagneticSensorSPI.cpp +++ b/src/MagneticSensorSPI.cpp @@ -233,7 +233,7 @@ word MagneticSensorSPI::read(word angle_register){ register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word - const static word data_mask = ~(0 >> (16 - bit_resolution)); + const static word data_mask = 0xFFFF >> (16 - bit_resolution); return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits } From efbad7d491a2bb09c5c8ea93e2df7eabcc12d877 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Sat, 24 Oct 2020 10:58:33 +0100 Subject: [PATCH 006/749] The angle register is incorrect 0x3FFF --- src/MagneticSensorSPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MagneticSensorSPI.cpp b/src/MagneticSensorSPI.cpp index 71acddb8..e2e2ff29 100644 --- a/src/MagneticSensorSPI.cpp +++ b/src/MagneticSensorSPI.cpp @@ -5,7 +5,7 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .spi_mode = SPI_MODE1, .clock_speed = 1000000, .bit_resolution = 14, - .angle_register = 0xCFFF, + .angle_register = 0x3FFF, .data_start_bit = 13, .command_rw_bit = 14, .command_parity_bit = 15 From 0d9156e22d438ebaa236dd74f8e04e4a8b09405e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 8 Nov 2020 19:23:03 +0100 Subject: [PATCH 007/749] Update library.properties --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 2481a306..32e06b8e 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=1.6.0 +version=1.6.1 author=Antun Skuric maintainer=Antun Skuric sentence=A library demistifying FOC for BLDC motors From 6c02b3417aa89e3908aa91bc461d2f380bc0f35a Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 12 Nov 2020 14:56:53 +0100 Subject: [PATCH 008/749] FEAT restrucutred hardware specific funcitons to separate files --- library.properties | 6 +- src/BLDCMotor.h | 4 +- src/Encoder.h | 2 +- src/HallSensor.h | 2 +- src/MagneticSensorAnalog.h | 2 +- src/MagneticSensorI2C.h | 2 +- src/MagneticSensorSPI.h | 2 +- src/StepperMotor.cpp | 22 +++- src/StepperMotor.h | 4 +- src/common/hardware_utils.h | 10 -- .../hardware_utils/arduino_specific.cpp | 93 ++++++++++++++++ src/common/hardware_utils/arm_specific.cpp | 66 ++++++++++++ .../esp32_specific.cpp} | 100 ++---------------- src/common/hardware_utils/stm32_specific.cpp | 66 ++++++++++++ src/common/{ => interfaces}/FOCMotor.cpp | 0 src/common/{ => interfaces}/FOCMotor.h | 26 +++-- src/common/{ => interfaces}/Sensor.h | 0 17 files changed, 285 insertions(+), 122 deletions(-) create mode 100644 src/common/hardware_utils/arduino_specific.cpp create mode 100644 src/common/hardware_utils/arm_specific.cpp rename src/common/{hardware_utils.cpp => hardware_utils/esp32_specific.cpp} (69%) create mode 100644 src/common/hardware_utils/stm32_specific.cpp rename src/common/{ => interfaces}/FOCMotor.cpp (100%) rename src/common/{ => interfaces}/FOCMotor.h (93%) rename src/common/{ => interfaces}/Sensor.h (100%) diff --git a/library.properties b/library.properties index 32e06b8e..4b72b9d3 100644 --- a/library.properties +++ b/library.properties @@ -1,7 +1,7 @@ name=Simple FOC -version=1.6.1 -author=Antun Skuric -maintainer=Antun Skuric +version=1.7.0 +author=Simplefoc +maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors paragraph=Simple library intended for hobby comunity to run the BLDC and Stepper motor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards. category=Device Control diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a4fbb678..31c70c69 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -7,10 +7,10 @@ #define BLDCMotor_h #include "Arduino.h" -#include "common/FOCMotor.h" +#include "common/interfaces/FOCMotor.h" +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" #include "common/defaults.h" /** diff --git a/src/Encoder.h b/src/Encoder.h index d1a55f0e..1e9ca65b 100644 --- a/src/Encoder.h +++ b/src/Encoder.h @@ -4,7 +4,7 @@ #include "Arduino.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" +#include "common/interfaces/Sensor.h" /** diff --git a/src/HallSensor.h b/src/HallSensor.h index 9b467b0a..9f7226b6 100644 --- a/src/HallSensor.h +++ b/src/HallSensor.h @@ -2,9 +2,9 @@ #define HALL_SENSOR_LIB_H #include "Arduino.h" +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" class HallSensor: public Sensor{ diff --git a/src/MagneticSensorAnalog.h b/src/MagneticSensorAnalog.h index cfe1b905..e2938dea 100644 --- a/src/MagneticSensorAnalog.h +++ b/src/MagneticSensorAnalog.h @@ -2,9 +2,9 @@ #define MAGNETICSENSORANALOG_LIB_H #include "Arduino.h" +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" /** * This sensor has been tested with AS5600 running in 'analog mode'. This is where output pin of AS6000 is connected to an analog pin on your microcontroller. diff --git a/src/MagneticSensorI2C.h b/src/MagneticSensorI2C.h index 88d74d35..ade14fff 100644 --- a/src/MagneticSensorI2C.h +++ b/src/MagneticSensorI2C.h @@ -3,9 +3,9 @@ #include "Arduino.h" #include +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" struct MagneticSensorI2CConfig_s { int chip_address; diff --git a/src/MagneticSensorSPI.h b/src/MagneticSensorSPI.h index 8e884cf4..2212b57f 100644 --- a/src/MagneticSensorSPI.h +++ b/src/MagneticSensorSPI.h @@ -3,9 +3,9 @@ #include "Arduino.h" #include +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" #define DEF_ANGLE_REGISTAR 0x3FFF diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 22ceb256..050bde75 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -179,7 +179,7 @@ void StepperMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle, pole_pairs)); } // Iterative function running outer loop of the FOC algorithm @@ -241,10 +241,24 @@ void StepperMotor::setPhaseVoltage(float Uq, float angle_el) { // only necessary if using _sin and _cos - approximation functions angle_el = _normalizeAngle(angle_el + zero_electric_angle); // Inverse park transform - Ualpha = -_sin(angle_el) * Uq; // -sin(angle) * Uq; - Ubeta = _cos(angle_el) * Uq; // cos(angle) * Uq; + Ualpha = -(_sin(angle_el)) * Uq; // -sin(angle) * Uq; + Ubeta = (_cos(angle_el)) * Uq; // cos(angle) * Uq; + // if (angle_el < _PI_2) { + // Ualpha = 0; + // Ubeta = Uq; + // }else if (angle_el < _PI) { + // Ualpha =- Uq; + // Ubeta = 0; + // }else if (angle_el < _3PI_2) { + // Ualpha = 0; + // Ubeta = -Uq; + // }else{ + // Ualpha = Uq; + // Ubeta = 0; + // } + // set the voltages in hardware - setPwm(Ualpha,Ubeta); + setPwm(Ualpha, Ubeta); } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index bf14e096..22cd148f 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -7,10 +7,10 @@ #define StepperMotor_h #include "Arduino.h" -#include "common/FOCMotor.h" +#include "common/interfaces/FOCMotor.h" +#include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" -#include "common/Sensor.h" #include "common/defaults.h" /** diff --git a/src/common/hardware_utils.h b/src/common/hardware_utils.h index f99c260a..3b70af1a 100644 --- a/src/common/hardware_utils.h +++ b/src/common/hardware_utils.h @@ -1,18 +1,8 @@ #ifndef HARDWARE_UTILS_H #define HARDWARE_UTILS_H -#include "Arduino.h" #include "foc_utils.h" - -#if defined(ESP_H) // if esp32 boards - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#endif - /** * High PWM frequency setting function * - hardware specific diff --git a/src/common/hardware_utils/arduino_specific.cpp b/src/common/hardware_utils/arduino_specific.cpp new file mode 100644 index 00000000..31cfec1c --- /dev/null +++ b/src/common/hardware_utils/arduino_specific.cpp @@ -0,0 +1,93 @@ +#include "../hardware_utils.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +#endif +} + + +// function setting the high pwm frequency to the supplied pins +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); + if(pinD != NOT_SET) _pinHighFrequency(pinD); // stepper motor +#endif +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} + +#endif \ No newline at end of file diff --git a/src/common/hardware_utils/arm_specific.cpp b/src/common/hardware_utils/arm_specific.cpp new file mode 100644 index 00000000..90d7679c --- /dev/null +++ b/src/common/hardware_utils/arm_specific.cpp @@ -0,0 +1,66 @@ +#include "../hardware_utils.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(pin, freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ + // regular micros + delay(ms); +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ + // regular micros + return micros(); +} + + +#endif \ No newline at end of file diff --git a/src/common/hardware_utils.cpp b/src/common/hardware_utils/esp32_specific.cpp similarity index 69% rename from src/common/hardware_utils.cpp rename to src/common/hardware_utils/esp32_specific.cpp index 43e2af6b..bc44c8f2 100644 --- a/src/common/hardware_utils.cpp +++ b/src/common/hardware_utils/esp32_specific.cpp @@ -1,6 +1,11 @@ -#include "hardware_utils.h" +#include "../hardware_utils.h" + +#if defined(ESP_H) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" -#if defined(ESP_H) // if ESP32 board // empty motor slot #define _EMPTY_SLOT -20 @@ -87,58 +92,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].sync.out_sel = 0; } -#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips -// set pwm frequency to 32KHz -void _pinHighFrequency(const int pin){ - // High PWM frequency - // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - } - if (pin == 9 || pin == 10 ) - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) - TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - -} -#elif defined(_STM32_DEF_) // if stm chips -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin){ - analogWrite(pin, 0); - analogWriteFrequency(freq); - analogWriteResolution(12); // resolution 12 bit 0 - 4096 -} -#elif defined(__arm__) && defined(CORE_TEENSY) //if teensy 3x / 4x / LC boards -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin){ - analogWrite(pin, 0); - analogWriteFrequency(pin, freq); -} -#endif - // function setting the high pwm frequency to the supplied pins // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); - _pinHighFrequency(pinC); - if(pinD != NOT_SET) _pinHighFrequency(pinD); // stepper motor - -#elif defined(_STM32_DEF_) || (defined(__arm__) && defined(CORE_TEENSY)) //if stm32 or teensy 3x / 4x / LC boards - if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz - else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor -#elif defined(ESP_H) // if esp32 boards if(pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency @@ -184,7 +143,6 @@ void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const i // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); } -#endif } // function setting the pwm duty cycle to the hardware @@ -193,7 +151,6 @@ void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const i // Arduino and STM32 devices use analogWrite() // ESP32 uses MCPWM void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ -#if defined(ESP_H) // if ESP32 boards // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_bldc_motor_slots[i].pinA == pinA){ // if motor slot found @@ -205,17 +162,6 @@ void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, in break; } } -#elif defined(_STM32_DEF_) // STM32 devices - // transform duty cycle from [0,1] to [0,4095] - analogWrite(pinA, 4095.0*dc_a); - analogWrite(pinB, 4095.0*dc_b); - analogWrite(pinC, 4095.0*dc_c); -#else // Arduino & Teensy - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255*dc_a); - analogWrite(pinB, 255*dc_b); - analogWrite(pinC, 255*dc_c); -#endif } // function setting the pwm duty cycle to the hardware @@ -224,7 +170,6 @@ void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, in // Arduino and STM32 devices use analogWrite() // ESP32 uses MCPWM void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ -#if defined(ESP_H) // if ESP32 boards // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found @@ -237,48 +182,23 @@ void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pi break; } } -#elif defined(_STM32_DEF_) // STM32 devices - // transform duty cycle from [0,1] to [0,4095] - analogWrite(pin1A, 4095.0*dc_1a); - analogWrite(pin1B, 4095.0*dc_1b); - analogWrite(pin2A, 4095.0*dc_2a); - analogWrite(pin2B, 4095.0*dc_2b); -#else // Arduino & Teensy - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255*dc_1a); - analogWrite(pin1B, 255*dc_1b); - analogWrite(pin2A, 255*dc_2a); - analogWrite(pin2B, 255*dc_2b); -#endif } // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) - // if arduino uno and other atmega328p chips - // use while instad of delay, - // due to wrong measurement based on changed timer0 - unsigned long t = _micros() + ms*1000; - while( _micros() < t ){}; -#else // regular micros delay(ms); -#endif } // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) -// if arduino uno and other atmega328p chips - //return the value based on the prescaler - if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); - else return (micros()); -#else // regular micros return micros(); -#endif } + + +#endif \ No newline at end of file diff --git a/src/common/hardware_utils/stm32_specific.cpp b/src/common/hardware_utils/stm32_specific.cpp new file mode 100644 index 00000000..6efc37bd --- /dev/null +++ b/src/common/hardware_utils/stm32_specific.cpp @@ -0,0 +1,66 @@ + +#include "../hardware_utils.h" + +#if defined(_STM32_DEF_) + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(freq); + analogWriteResolution(12); // resolution 12 bit 0 - 4096 +} + + +// function setting the high pwm frequency to the supplied pins +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + analogWrite(pinA, 4095.0*dc_a); + analogWrite(pinB, 4095.0*dc_b); + analogWrite(pinC, 4095.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + analogWrite(pin1A, 4095.0*dc_1a); + analogWrite(pin1B, 4095.0*dc_1b); + analogWrite(pin2A, 4095.0*dc_2a); + analogWrite(pin2B, 4095.0*dc_2b); +} + + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ + // regular micros + delay(ms); +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ + // regular micros + return micros(); +} +#endif \ No newline at end of file diff --git a/src/common/FOCMotor.cpp b/src/common/interfaces/FOCMotor.cpp similarity index 100% rename from src/common/FOCMotor.cpp rename to src/common/interfaces/FOCMotor.cpp diff --git a/src/common/FOCMotor.h b/src/common/interfaces/FOCMotor.h similarity index 93% rename from src/common/FOCMotor.h rename to src/common/interfaces/FOCMotor.h index 40f7f6d4..676089b5 100644 --- a/src/common/FOCMotor.h +++ b/src/common/interfaces/FOCMotor.h @@ -2,13 +2,14 @@ #define FOCMOTOR_H #include "Arduino.h" -#include "hardware_utils.h" -#include "foc_utils.h" -#include "defaults.h" - #include "Sensor.h" -#include "pid.h" -#include "lowpass_filter.h" +#include "FOCDriver.h" + +#include "../hardware_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" /** @@ -55,6 +56,13 @@ class FOCMotor */ void linkSensor(Sensor* sensor); + /** + * Function linking a motor and a foc driver + * + * @param driver FOCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(FOCDriver* driver); + /** * Function initializing FOC algorithm * and aligning sensor's and motors' zero position @@ -187,6 +195,12 @@ class FOCMotor * - HallSensor */ Sensor* sensor; + /** + * FOCDriver link: + * - 3PWM + * - 6PWM + */ + FOCDriver* driver; // monitoring functions Print* monitor_port; //!< Serial terminal variable if provided }; diff --git a/src/common/Sensor.h b/src/common/interfaces/Sensor.h similarity index 100% rename from src/common/Sensor.h rename to src/common/interfaces/Sensor.h From fae63ecf4f1a9c6b5f1518711ccc7e716d35ed1c Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 12 Nov 2020 15:01:09 +0100 Subject: [PATCH 009/749] FIX forgot to remoive FOCDriver --- src/common/interfaces/FOCDriver.h | 18 ++++++++++++++++++ src/common/interfaces/FOCMotor.h | 14 -------------- 2 files changed, 18 insertions(+), 14 deletions(-) create mode 100644 src/common/interfaces/FOCDriver.h diff --git a/src/common/interfaces/FOCDriver.h b/src/common/interfaces/FOCDriver.h new file mode 100644 index 00000000..50c9d4be --- /dev/null +++ b/src/common/interfaces/FOCDriver.h @@ -0,0 +1,18 @@ +#ifndef FOCDRIVER_H +#define FOCDRIVER_H + +class FOCDriver{ + public: + /** Initialise hardware */ + virtual void init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + + /** Set the pwm frequency */ + virtual void setPwmFrequency(long frequency); +}; + +#endif \ No newline at end of file diff --git a/src/common/interfaces/FOCMotor.h b/src/common/interfaces/FOCMotor.h index 676089b5..407188f2 100644 --- a/src/common/interfaces/FOCMotor.h +++ b/src/common/interfaces/FOCMotor.h @@ -3,7 +3,6 @@ #include "Arduino.h" #include "Sensor.h" -#include "FOCDriver.h" #include "../hardware_utils.h" #include "../foc_utils.h" @@ -56,13 +55,6 @@ class FOCMotor */ void linkSensor(Sensor* sensor); - /** - * Function linking a motor and a foc driver - * - * @param driver FOCDriver class implementing all the hardware specific functions necessary PWM setting - */ - void linkDriver(FOCDriver* driver); - /** * Function initializing FOC algorithm * and aligning sensor's and motors' zero position @@ -195,12 +187,6 @@ class FOCMotor * - HallSensor */ Sensor* sensor; - /** - * FOCDriver link: - * - 3PWM - * - 6PWM - */ - FOCDriver* driver; // monitoring functions Print* monitor_port; //!< Serial terminal variable if provided }; From 3da53b9fd682326b47a99ef67dadc3b0d7b00c59 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 12 Nov 2020 18:12:11 +0100 Subject: [PATCH 010/749] FEAT added separate driver classes --- keywords.txt | 7 +- src/BLDCDriver3PWM.cpp | 69 ++++++++++++++ src/BLDCDriver3PWM.h | 51 +++++++++++ src/BLDCMotor.cpp | 91 ++++++++----------- src/BLDCMotor.h | 47 +++++----- src/SimpleFOC.h | 2 + src/StepperDriver4PWM.cpp | 79 ++++++++++++++++ src/StepperDriver4PWM.h | 54 +++++++++++ src/StepperMotor.cpp | 69 ++++---------- src/StepperMotor.h | 44 ++++----- src/common/foc_utils.h | 1 + .../arduino_specific.cpp | 0 .../arm_specific.cpp | 4 +- .../esp32_specific.cpp | 4 +- .../stm32_specific.cpp | 4 +- src/common/interfaces/BLDCDriver.h | 28 ++++++ src/common/interfaces/FOCDriver.h | 18 ---- src/common/interfaces/FOCMotor.cpp | 3 - src/common/interfaces/FOCMotor.h | 6 +- src/common/interfaces/StepperDriver.h | 27 ++++++ src/common/pid.cpp | 4 +- 21 files changed, 427 insertions(+), 185 deletions(-) create mode 100644 src/BLDCDriver3PWM.cpp create mode 100644 src/BLDCDriver3PWM.h create mode 100644 src/StepperDriver4PWM.cpp create mode 100644 src/StepperDriver4PWM.h rename src/common/{hardware_utils => hardware_specific}/arduino_specific.cpp (100%) rename src/common/{hardware_utils => hardware_specific}/arm_specific.cpp (90%) rename src/common/{hardware_utils => hardware_specific}/esp32_specific.cpp (96%) rename src/common/{hardware_utils => hardware_specific}/stm32_specific.cpp (91%) create mode 100644 src/common/interfaces/BLDCDriver.h delete mode 100644 src/common/interfaces/FOCDriver.h create mode 100644 src/common/interfaces/StepperDriver.h diff --git a/keywords.txt b/keywords.txt index ebc4bc12..a1eee83e 100644 --- a/keywords.txt +++ b/keywords.txt @@ -8,6 +8,10 @@ HallSensors KEYWORD1 MagneticSensorSPI KEYWORD1 MagneticSensorI2C KEYWORD1 MagneticSensorAnalog KEYWORD1 +BLDCDriver3PWM KEYWORD1 +BLDCDriver KEYWORD1 +StepperDriver4PWM KEYWORD1 +StepperDriver KEYWORD1 initFOC KEYWORD2 loopFOC KEYWORD2 @@ -36,6 +40,7 @@ Direction KEYWORD1 MagneticSensorI2CConfig_s KEYWORD1 MagneticSensorSPIConfig_s KEYWORD1 +linkDriver KEYWORD2 linkSensor KEYWORD2 handleA KEYWORD2 handleB KEYWORD2 @@ -60,11 +65,11 @@ shaft_velocity_sp KEYWORD2 shaft_angle KEYWORD2 shaft_velocity KEYWORD2 controller KEYWORD2 -driver KEYWORD2 pullup KEYWORD2 quadrature KEYWORD2 foc_modulation KEYWORD2 target KEYWORD2 +pwm_frequency KEYWORD2 voltage KEYWORD2 diff --git a/src/BLDCDriver3PWM.cpp b/src/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..9c0e27f7 --- /dev/null +++ b/src/BLDCDriver3PWM.cpp @@ -0,0 +1,69 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +void BLDCDriver3PWM::init() { + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _setPwmFrequency(pwm_frequency, pwmA, pwmB, pwmC); +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, -voltage_limit, voltage_limit); + Ub = _constrain(Ub, -voltage_limit, voltage_limit); + Uc = _constrain(Uc, -voltage_limit, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/src/BLDCDriver3PWM.h b/src/BLDCDriver3PWM.h new file mode 100644 index 00000000..b05b9852 --- /dev/null +++ b/src/BLDCDriver3PWM.h @@ -0,0 +1,51 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "common/interfaces/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/hardware_utils.h" +#include "common/defaults.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 989b74a9..cc248dce 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -5,37 +5,35 @@ // - pp - pole pair number // - cpr - counts per rotation number (cpm=ppm*4) // - enable pin - (optional input) -BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en) +BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ + +} + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) : FOCMotor() { - // Pin initialization - pwmA = phA; - pwmB = phB; - pwmC = phC; + // save pole pairs number pole_pairs = pp; +} - // enable_pin pin - enable_pin = en; +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; } - // init hardware pins -void BLDCMotor::init(long pwm_frequency) { - if(monitor_port) monitor_port->println("MOT: Init pins."); - // PWM pins - pinMode(pwmA, OUTPUT); - pinMode(pwmB, OUTPUT); - pinMode(pwmC, OUTPUT); - if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); - - if(monitor_port) monitor_port->println("MOT: PWM config."); - // Increase PWM frequency to 32 kHz - // make silent - _setPwmFrequency(pwm_frequency, pwmA, pwmB, pwmC); - +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); // sanity check for the voltage limit configuration - if(voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if(voltage_limit > driver->voltage_power_supply) voltage_limit = driver->voltage_power_supply; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; // update the controller limits @@ -44,7 +42,7 @@ void BLDCMotor::init(long pwm_frequency) { _delay(500); // enable motor - if(monitor_port) monitor_port->println("MOT: Enable."); + if(monitor_port) monitor_port->println("MOT: Enable driver."); enable(); _delay(500); } @@ -53,18 +51,18 @@ void BLDCMotor::init(long pwm_frequency) { // disable motor driver void BLDCMotor::disable() { - // disable the driver - if enable_pin pin available - if (enable_pin != NOT_SET) digitalWrite(enable_pin, LOW); // set zero to PWM - setPwm(0, 0, 0); + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); } // enable motor driver void BLDCMotor::enable() { + // enable the driver + driver->enable(); // set zero to PWM - setPwm(0, 0, 0); - // enable_pin the driver - if enable_pin pin available - if (enable_pin != NOT_SET) digitalWrite(enable_pin, HIGH); + driver->setPwm(0, 0, 0); } @@ -240,9 +238,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { Ubeta = _cos(angle_el) * Uq; // cos(angle) * Uq; // Clarke transform - Ua = Ualpha + voltage_power_supply/2; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + voltage_power_supply/2; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + voltage_power_supply/2; + Ua = Ualpha + driver->voltage_power_supply/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; break; case FOCModulationType::SpaceVectorPWM : @@ -261,10 +259,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { // find the sector we are in currently int sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/voltage_power_supply; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/voltage_power_supply; + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply; // two versions possible - // centered around voltage_power_supply/2 + // centered around driver->voltage_power_supply/2 float T0 = 1 - T1 - T2; // pulled to 0 - better for low power supply voltage //float T0 = 0; @@ -310,27 +308,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { } // calculate the phase voltages and center - Ua = Ta*voltage_power_supply; - Ub = Tb*voltage_power_supply; - Uc = Tc*voltage_power_supply; + Ua = Ta*driver->voltage_power_supply; + Ub = Tb*driver->voltage_power_supply; + Uc = Tc*driver->voltage_power_supply; break; } - // set the voltages in hardware - setPwm(Ua, Ub, Uc); -} - - - -// Set voltage to the pwm pin -void BLDCMotor::setPwm(float Ua, float Ub, float Uc) { - // calculate duty cycle - // limited in [0,1] - float dc_a = constrain(Ua / voltage_power_supply, 0 , 1 ); - float dc_b = constrain(Ub / voltage_power_supply, 0 , 1 ); - float dc_c = constrain(Uc / voltage_power_supply, 0 , 1 ); - // hardware specific writing - _writeDutyCycle(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC ); + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); } diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 31c70c69..48e48f62 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -9,6 +9,7 @@ #include "Arduino.h" #include "common/interfaces/FOCMotor.h" #include "common/interfaces/Sensor.h" +#include "common/interfaces/BLDCDriver.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" #include "common/defaults.h" @@ -20,18 +21,28 @@ class BLDCMotor: public FOCMotor { public: /** - BLDCMotor class constructor - @param phA A phase pwm pin - @param phB B phase pwm pin - @param phC C phase pwm pin - @param pp pole pair number - @param cpr counts per rotation number (cpm=ppm*4) - @param en enable pin (optional input) + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM */ - BLDCMotor(int phA,int phB,int phC,int pp, int en = NOT_SET); + BLDCDriver* driver; /** Motor hardware init function */ - void init(long pwm_frequency = NOT_SET) override; + void init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -59,12 +70,9 @@ class BLDCMotor: public FOCMotor * This function doesn't need to be run upon each loop execution - depends of the use case */ void move(float target = NOT_SET) override; - - // hardware variables - int pwmA; //!< phase A pwm pin number - int pwmB; //!< phase B pwm pin number - int pwmC; //!< phase C pwm pin number - int enable_pin; //!< enable pin number + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform private: @@ -81,14 +89,7 @@ class BLDCMotor: public FOCMotor int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroAlign(); - /** - * Set phase voltages to the harware - * - * @param Ua phase A voltage - * @param Ub phase B voltage - * @param Uc phase C voltage - */ - void setPwm(float Ua, float Ub, float Uc); + // Open loop motion control /** diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 5925e78d..9d1e9225 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -93,5 +93,7 @@ void loop() { #include "MagneticSensorI2C.h" #include "MagneticSensorAnalog.h" #include "HallSensor.h" +#include "BLDCDriver3PWM.h" +#include "StepperDriver4PWM.h" #endif diff --git a/src/StepperDriver4PWM.cpp b/src/StepperDriver4PWM.cpp new file mode 100644 index 00000000..379d9c2d --- /dev/null +++ b/src/StepperDriver4PWM.cpp @@ -0,0 +1,79 @@ +#include "StepperDriver4PWM.h" + +StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1, int en2){ + // Pin initialization + pwm1A = ph1A; + pwm1B = ph1B; + pwm2A = ph2A; + pwm2B = ph2B; + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver4PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver4PWM::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +void StepperDriver4PWM::init() { + + // PWM pins + pinMode(pwm1A, OUTPUT); + pinMode(pwm1B, OUTPUT); + pinMode(pwm2A, OUTPUT); + pinMode(pwm2B, OUTPUT); + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _setPwmFrequency(pwm_frequency, pwm1A, pwm2A, pwm1B, pwm2B); +} + + +// Set voltage to the pwm pin +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha,-voltage_limit,voltage_limit); + Ubeta = _constrain(Ubeta,-voltage_limit,voltage_limit); + // hardware specific writing + if( Ualpha > 0 ) + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0,1); + else + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0,1); + + if( Ubeta > 0 ) + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0,1); + else + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0,1); + // write to hardware + _writeDutyCycle(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); +} \ No newline at end of file diff --git a/src/StepperDriver4PWM.h b/src/StepperDriver4PWM.h new file mode 100644 index 00000000..5711bb2c --- /dev/null +++ b/src/StepperDriver4PWM.h @@ -0,0 +1,54 @@ +#ifndef STEPPER_DRIVER_4PWM_h +#define STEPPER_DRIVER_4PWM_h + +#include "common/interfaces/StepperDriver.h" +#include "common/foc_utils.h" +#include "common/hardware_utils.h" +#include "common/defaults.h" + +/** + 4 pwm stepper driver class +*/ +class StepperDriver4PWM: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1A 1A phase pwm pin + @param ph1B 1B phase pwm pin + @param ph2A 2A phase pwm pin + @param ph2B 2B phase pwm pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1A; //!< phase 1A pwm pin number + int pwm1B; //!< phase 1B pwm pin number + int pwm2A; //!< phase 2A pwm pin number + int pwm2B; //!< phase 2B pwm pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 050bde75..590a895d 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -5,36 +5,23 @@ // - ph2A, ph2B - motor phase 2 pwm pins // - pp - pole pair number // - enable pin - (optional input) -StepperMotor::StepperMotor(int ph1A, int ph1B, int ph2A, int ph2B, int pp, int en1, int en2) +StepperMotor::StepperMotor(int pp) : FOCMotor() { - // Pin initialization - pwm1A = ph1A; - pwm1B = ph1B; - pwm2A = ph2A; - pwm2B = ph2B; + // number od pole pairs pole_pairs = pp; +} - // enable_pin pin - enable_pin1 = en1; - enable_pin2 = en2; +/** + Link the driver which controls the motor +*/ +void StepperMotor::linkDriver(StepperDriver* _driver) { + driver = _driver; } // init hardware pins -void StepperMotor::init(long pwm_frequency) { - if(monitor_port) monitor_port->println("MOT: Init pins."); - // PWM pins - pinMode(pwm1A, OUTPUT); - pinMode(pwm1B, OUTPUT); - pinMode(pwm2A, OUTPUT); - pinMode(pwm2B, OUTPUT); - if ( enable_pin1 != NOT_SET ) pinMode(enable_pin1, OUTPUT); - if ( enable_pin2 != NOT_SET ) pinMode(enable_pin2, OUTPUT); - - if(monitor_port) monitor_port->println("MOT: PWM config."); - // Increase PWM frequency - // make silent - _setPwmFrequency(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); +void StepperMotor::init() { + if(monitor_port) monitor_port->println("MOT: Init variables."); // sanity check for the voltage limit configuration if(voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; @@ -57,21 +44,18 @@ void StepperMotor::init(long pwm_frequency) { // disable motor driver void StepperMotor::disable() { - // disable the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); // set zero to PWM - setPwm(0, 0); + driver->setPwm(0, 0); + // disable driver + driver->disable(); } // enable motor driver void StepperMotor::enable() { + // disable enable + driver->enable(); // set zero to PWM - setPwm(0, 0); - // enable_pin the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); - + driver->setPwm(0, 0); } @@ -258,26 +242,7 @@ void StepperMotor::setPhaseVoltage(float Uq, float angle_el) { // } // set the voltages in hardware - setPwm(Ualpha, Ubeta); -} - - - -// Set voltage to the pwm pin -void StepperMotor::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); - // hardware specific writing - if( Ualpha > 0 ) - duty_cycle1B = constrain(abs(Ualpha)/voltage_power_supply,0,1); - else - duty_cycle1A = constrain(abs(Ualpha)/voltage_power_supply,0,1); - - if( Ubeta > 0 ) - duty_cycle2B = constrain(abs(Ubeta)/voltage_power_supply,0,1); - else - duty_cycle2A = constrain(abs(Ubeta)/voltage_power_supply,0,1); - // write to hardware - _writeDutyCycle(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); + driver->setPwm(Ualpha, Ubeta); } // Function (iterative) generating open loop movement for target velocity diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 22cd148f..70418a6b 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -8,6 +8,7 @@ #include "Arduino.h" #include "common/interfaces/FOCMotor.h" +#include "common/interfaces/StepperDriver.h" #include "common/interfaces/Sensor.h" #include "common/foc_utils.h" #include "common/hardware_utils.h" @@ -21,18 +22,27 @@ class StepperMotor: public FOCMotor public: /** StepperMotor class constructor - @param ph1A 1A phase pwm pin - @param ph1B 1B phase pwm pin - @param ph2A 2A phase pwm pin - @param ph2B 2B phase pwm pin @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) - @param en1 enable pin phase 1 (optional input) - @param en2 enable pin phase 2 (optional input) */ - StepperMotor(int ph1A,int ph1B,int ph2A,int ph2B,int pp, int en1 = NOT_SET, int en2 = NOT_SET); + StepperMotor(int pp); + + + /** + * Function linking a motor and a foc driver + * + * @param driver StepperDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(StepperDriver* driver); + + /** + * StepperDriver link: + * - 4PWM - L298N for example + */ + StepperDriver* driver; + /** Motor hardware init function */ - void init(long pwm_frequency = NOT_SET) override; + void init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -65,14 +75,8 @@ class StepperMotor: public FOCMotor * This function doesn't need to be run upon each loop execution - depends of the use case */ void move(float target = NOT_SET) override; - - // hardware variables - int pwm1A; //!< phase 1A pwm pin number - int pwm1B; //!< phase 1B pwm pin number - int pwm2A; //!< phase 2A pwm pin number - int pwm2B; //!< phase 2B pwm pin number - int enable_pin1; //!< enable pin number phase 1 - int enable_pin2; //!< enable pin number phase 2 + + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform private: @@ -90,14 +94,6 @@ class StepperMotor: public FOCMotor int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroAlign(); - /** - * Set phase voltages to the harware - * - * @param Ua phase A voltage - * @param Ub phase B voltage - * @param Uc phase C voltage - */ - void setPwm(float Ualpha, float Ubeta); // Open loop motion control /** diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 5ab34f42..155528b5 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -6,6 +6,7 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) #define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) // utility defines #define _2_SQRT3 1.15470053838 diff --git a/src/common/hardware_utils/arduino_specific.cpp b/src/common/hardware_specific/arduino_specific.cpp similarity index 100% rename from src/common/hardware_utils/arduino_specific.cpp rename to src/common/hardware_specific/arduino_specific.cpp diff --git a/src/common/hardware_utils/arm_specific.cpp b/src/common/hardware_specific/arm_specific.cpp similarity index 90% rename from src/common/hardware_utils/arm_specific.cpp rename to src/common/hardware_specific/arm_specific.cpp index 90d7679c..8753d980 100644 --- a/src/common/hardware_utils/arm_specific.cpp +++ b/src/common/hardware_specific/arm_specific.cpp @@ -13,8 +13,8 @@ void _setHighFrequency(const long freq, const int pin){ // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz - else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); diff --git a/src/common/hardware_utils/esp32_specific.cpp b/src/common/hardware_specific/esp32_specific.cpp similarity index 96% rename from src/common/hardware_utils/esp32_specific.cpp rename to src/common/hardware_specific/esp32_specific.cpp index bc44c8f2..f040bd9f 100644 --- a/src/common/hardware_utils/esp32_specific.cpp +++ b/src/common/hardware_specific/esp32_specific.cpp @@ -98,8 +98,8 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // supports Arudino/ATmega328, STM32 and ESP32 void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency if(pinD == NOT_SET){ bldc_motor_slots_t m_slot = {}; diff --git a/src/common/hardware_utils/stm32_specific.cpp b/src/common/hardware_specific/stm32_specific.cpp similarity index 91% rename from src/common/hardware_utils/stm32_specific.cpp rename to src/common/hardware_specific/stm32_specific.cpp index 6efc37bd..41fbf764 100644 --- a/src/common/hardware_utils/stm32_specific.cpp +++ b/src/common/hardware_specific/stm32_specific.cpp @@ -15,8 +15,8 @@ void _setHighFrequency(const long freq, const int pin){ // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz - else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); diff --git a/src/common/interfaces/BLDCDriver.h b/src/common/interfaces/BLDCDriver.h new file mode 100644 index 00000000..f53488d2 --- /dev/null +++ b/src/common/interfaces/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual void init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/src/common/interfaces/FOCDriver.h b/src/common/interfaces/FOCDriver.h deleted file mode 100644 index 50c9d4be..00000000 --- a/src/common/interfaces/FOCDriver.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef FOCDRIVER_H -#define FOCDRIVER_H - -class FOCDriver{ - public: - /** Initialise hardware */ - virtual void init(); - /** Enable hardware */ - virtual void enable(); - /** Disable hardware */ - virtual void disable(); - - - /** Set the pwm frequency */ - virtual void setPwmFrequency(long frequency); -}; - -#endif \ No newline at end of file diff --git a/src/common/interfaces/FOCMotor.cpp b/src/common/interfaces/FOCMotor.cpp index adecbeec..591463aa 100644 --- a/src/common/interfaces/FOCMotor.cpp +++ b/src/common/interfaces/FOCMotor.cpp @@ -53,9 +53,6 @@ float FOCMotor::shaftVelocity() { return LPF_velocity(sensor->getVelocity()); } - - - /** * Monitoring functions */ diff --git a/src/common/interfaces/FOCMotor.h b/src/common/interfaces/FOCMotor.h index 407188f2..4283e2c9 100644 --- a/src/common/interfaces/FOCMotor.h +++ b/src/common/interfaces/FOCMotor.h @@ -3,6 +3,7 @@ #include "Arduino.h" #include "Sensor.h" +#include "BLDCDriver.h" #include "../hardware_utils.h" #include "../foc_utils.h" @@ -42,7 +43,7 @@ class FOCMotor FOCMotor(); /** Motor hardware init function */ - virtual void init(long pwm_frequency)=0; + virtual void init()=0; /** Motor disable function */ virtual void disable()=0; /** Motor enable function */ @@ -55,6 +56,7 @@ class FOCMotor */ void linkSensor(Sensor* sensor); + /** * Function initializing FOC algorithm * and aligning sensor's and motors' zero position @@ -99,8 +101,6 @@ class FOCMotor float shaft_velocity_sp;//!< current target velocity float shaft_angle_sp;//!< current target angle float voltage_q;//!< current voltage u_q set - float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform // motor configuration parameters float voltage_power_supply;//!< Power supply voltage diff --git a/src/common/interfaces/StepperDriver.h b/src/common/interfaces/StepperDriver.h new file mode 100644 index 00000000..17ea5c55 --- /dev/null +++ b/src/common/interfaces/StepperDriver.h @@ -0,0 +1,27 @@ +#ifndef STEPPERDRIVER_H +#define STEPPERDRIVER_H + +class StepperDriver{ + public: + + /** Initialise hardware */ + virtual void init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + virtual void setPwm(float Ua, float Ub); +}; + +#endif \ No newline at end of file diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 277e17ce..5275c728 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -30,7 +30,7 @@ float PIDController::operator() (float error){ // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) float integral = integral_prev + I*Ts*0.5*(error + error_prev); // antiwindup - limit the output voltage_q - integral = constrain(integral, -limit, limit); + integral = _constrain(integral, -limit, limit); // Discrete derivation // u_dk = D(ek - ek_1)/Ts float derivative = D*(error - error_prev)/Ts; @@ -38,7 +38,7 @@ float PIDController::operator() (float error){ // sum all the components float output = proportional + integral + derivative; // antiwindup - limit the output variable - output = constrain(output, -limit, limit); + output = _constrain(output, -limit, limit); // limit the acceleration by ramping the output float output_rate = (output - output_prev)/Ts; From aded2860a06d44cac62c859cb6635ad42289983a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 13 Nov 2020 17:40:29 +0100 Subject: [PATCH 011/749] FEAT added support for 6pwm for arduino and stm32 + refactured examples --- .../bluepill_position_control.ino | 16 +- .../bluepill_position_control.ino | 15 +- .../full_control_serial.ino | 18 +- .../full_control_serial.ino | 152 ++++++++++ .../esp32_position_control.ino | 14 +- .../esp32_position_control.ino | 12 +- .../position_control/position_control.ino | 14 +- .../voltage_control/voltage_control.ino | 14 +- .../open_loop_position_example.ino | 21 +- .../open_loop_velocity_example.ino | 20 +- .../encoder/angle_control/angle_control.ino | 17 +- .../angle_control/angle_control.ino | 16 +- .../angle_control/angle_control.ino | 19 +- .../voltage_control/voltage_control.ino | 23 +- .../voltage_control/voltage_control.ino | 19 +- .../voltage_control/voltage_control.ino | 22 +- .../velocity_control/velocity_control.ino | 17 +- .../hall_sensor/velocity_control.ino | 17 +- .../velocity_control/velocity_control.ino | 19 +- .../full_control_serial.ino | 22 +- .../full_control_serial.ino | 22 +- .../full_control_serial.ino | 21 +- .../alignment_and_cogging_test.ino | 12 +- .../find_pole_pairs_number.ino | 19 +- .../find_pole_pairs_number.ino | 22 +- .../find_sensor_offset_and_direction.ino | 15 +- keywords.txt | 3 + src/BLDCMotor.cpp | 16 +- src/BLDCMotor.h | 14 +- src/SimpleFOC.h | 15 +- src/StepperMotor.h | 10 +- .../{interfaces => base_classes}/BLDCDriver.h | 2 +- .../{interfaces => base_classes}/FOCMotor.cpp | 0 .../{interfaces => base_classes}/FOCMotor.h | 2 +- .../{interfaces => base_classes}/Sensor.h | 0 .../StepperDriver.h | 2 +- src/common/foc_utils.h | 1 - .../hardware_specific/arduino_specific.cpp | 93 ------ src/common/hardware_specific/arm_specific.cpp | 66 ----- .../hardware_specific/stm32_specific.cpp | 66 ----- src/common/hardware_utils.h | 62 ---- src/common/lowpass_filter.h | 2 +- src/common/pid.h | 2 +- src/common/time_utils.cpp | 31 ++ src/common/time_utils.h | 22 ++ src/{ => drivers}/BLDCDriver3PWM.cpp | 9 +- src/{ => drivers}/BLDCDriver3PWM.h | 11 +- src/drivers/BLDCDriver6PWM.cpp | 78 +++++ src/drivers/BLDCDriver6PWM.h | 57 ++++ src/{ => drivers}/StepperDriver4PWM.cpp | 8 +- src/{ => drivers}/StepperDriver4PWM.h | 11 +- src/drivers/hardware_api.h | 99 +++++++ src/drivers/hardware_specific/arm_mcu.cpp | 71 +++++ .../hardware_specific/atmega328_mcu.cpp | 133 +++++++++ .../hardware_specific/esp32_mcu.cpp} | 133 ++++----- src/drivers/hardware_specific/generic_mcu.cpp | 68 +++++ src/drivers/hardware_specific/stm32_mcu.cpp | 271 ++++++++++++++++++ src/{ => sensors}/Encoder.cpp | 0 src/{ => sensors}/Encoder.h | 6 +- src/{ => sensors}/HallSensor.cpp | 0 src/{ => sensors}/HallSensor.h | 6 +- src/{ => sensors}/MagneticSensorAnalog.cpp | 0 src/{ => sensors}/MagneticSensorAnalog.h | 6 +- src/{ => sensors}/MagneticSensorI2C.cpp | 0 src/{ => sensors}/MagneticSensorI2C.h | 6 +- src/{ => sensors}/MagneticSensorSPI.cpp | 0 src/{ => sensors}/MagneticSensorSPI.h | 6 +- 67 files changed, 1420 insertions(+), 566 deletions(-) rename examples/hardware_specific_examples/{DRV8305_driver/motor_full_control_serial_examples => DRV8302_driver/3pwm_example}/encoder/full_control_serial/full_control_serial.ino (93%) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino rename src/common/{interfaces => base_classes}/BLDCDriver.h (96%) rename src/common/{interfaces => base_classes}/FOCMotor.cpp (100%) rename src/common/{interfaces => base_classes}/FOCMotor.h (99%) rename src/common/{interfaces => base_classes}/Sensor.h (100%) rename src/common/{interfaces => base_classes}/StepperDriver.h (95%) delete mode 100644 src/common/hardware_specific/arduino_specific.cpp delete mode 100644 src/common/hardware_specific/arm_specific.cpp delete mode 100644 src/common/hardware_specific/stm32_specific.cpp delete mode 100644 src/common/hardware_utils.h create mode 100644 src/common/time_utils.cpp create mode 100644 src/common/time_utils.h rename src/{ => drivers}/BLDCDriver3PWM.cpp (90%) rename src/{ => drivers}/BLDCDriver3PWM.h (83%) create mode 100644 src/drivers/BLDCDriver6PWM.cpp create mode 100644 src/drivers/BLDCDriver6PWM.h rename src/{ => drivers}/StepperDriver4PWM.cpp (91%) rename src/{ => drivers}/StepperDriver4PWM.h (86%) create mode 100644 src/drivers/hardware_api.h create mode 100644 src/drivers/hardware_specific/arm_mcu.cpp create mode 100644 src/drivers/hardware_specific/atmega328_mcu.cpp rename src/{common/hardware_specific/esp32_specific.cpp => drivers/hardware_specific/esp32_mcu.cpp} (65%) create mode 100644 src/drivers/hardware_specific/generic_mcu.cpp create mode 100644 src/drivers/hardware_specific/stm32_mcu.cpp rename src/{ => sensors}/Encoder.cpp (100%) rename src/{ => sensors}/Encoder.h (96%) rename src/{ => sensors}/HallSensor.cpp (100%) rename src/{ => sensors}/HallSensor.h (96%) rename src/{ => sensors}/MagneticSensorAnalog.cpp (100%) rename src/{ => sensors}/MagneticSensorAnalog.h (95%) rename src/{ => sensors}/MagneticSensorI2C.cpp (100%) rename src/{ => sensors}/MagneticSensorI2C.h (96%) rename src/{ => sensors}/MagneticSensorSPI.cpp (100%) rename src/{ => sensors}/MagneticSensorSPI.h (96%) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index e112713e..3a850745 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -7,8 +7,12 @@ */ #include -// motor instance -BLDCMotor motor = BLDCMotor(PB6, PB7, PB8, 11, PB5); +// Motor instance +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver3PWM(IN1, IN2, IN3, enable(optional)) +BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5); +// BLDCDriver6PWM(IN1_H, IN1_L, IN2_H, IN2_L, IN3_H, IN3_L, enable(optional)) +//BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, PB12); // encoder instance Encoder encoder = Encoder(PA8, PA9, 8192, PA10); @@ -24,12 +28,16 @@ void setup() { // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB, doI); - // link the motor to the sensor motor.linkSensor(&encoder); + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] 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 d6cfc8d0..b80b614a 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 @@ -20,7 +20,11 @@ MagneticSensorSPI sensor = MagneticSensorSPI(PA4, 14, 0x3FFF); //MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); // Motor instance -BLDCMotor motor = BLDCMotor(PA3, PA2, PA1, 11, PA0); +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver3PWM(IN1, IN2, IN3, enable(optional)) +BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5); +// BLDCDriver6PWM(IN1_H, IN1_L, IN2_H, IN2_L, IN3_H, IN3_L, enable(optional)) +//BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, PB12); void setup() { @@ -29,9 +33,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/hardware_specific_examples/DRV8305_driver/motor_full_control_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino similarity index 93% rename from examples/hardware_specific_examples/DRV8305_driver/motor_full_control_serial_examples/encoder/full_control_serial/full_control_serial.ino rename to examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 14a73e02..ed0404d7 100644 --- a/examples/hardware_specific_examples/DRV8305_driver/motor_full_control_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -18,13 +18,15 @@ #define INH_A 9 #define INH_B 10 #define INH_C 11 + #define EN_GATE 7 #define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 -// motor instance -BLDCMotor motor = BLDCMotor(INH_A, INH_B, INH_C, 11, EN_GATE); +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); // encoder instance Encoder encoder = Encoder(2, 3, 8192); @@ -54,11 +56,17 @@ void setup() { pinMode(OC_ADJ,OUTPUT); digitalWrite(OC_ADJ,HIGH); - // choose FOC modulation - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = ControlType::voltage; diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino new file mode 100644 index 00000000..3de0f4d7 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -0,0 +1,152 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A 3 +#define INH_B 5 +#define INH_C 9 +#define INL_A 11 +#define INL_B 6 +#define INL_C 10 + +#define EN_GATE 7 +#define M_PWM A1 +#define M_OC A2 +#define OC_ADJ A3 + +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE); + +// encoder instance +Encoder encoder = Encoder(2, 3, 8192); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - disable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM, LOW); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set control loop type to be used + motor.controller = ControlType::voltage; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + Serial.println("Full control example: "); + Serial.println("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "); + Serial.println("Initial motion control loop is voltage loop."); + Serial.println("Initial target voltage 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} + diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 422b577a..0fe695d1 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -4,8 +4,9 @@ */ #include -// motor instance -BLDCMotor motor = BLDCMotor(25, 26, 27, 7); +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7); // encoder instance Encoder encoder = Encoder(4, 2, 1024); @@ -23,9 +24,14 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); - + + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index dc9855fa..ed5dbef9 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -19,7 +19,8 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); // Motor instance -BLDCMotor motor = BLDCMotor(25, 26, 27, 7); +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7); void setup() { @@ -28,9 +29,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index b8bf6137..5235c624 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -17,8 +17,9 @@ #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11); // encoder instance Encoder encoder = Encoder(A0, A1, 2048); @@ -41,9 +42,14 @@ void setup() { PciManager.registerListener(&listenerB); // link the motor to the sensor motor.linkSensor(&encoder); - + + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index 2a7e4987..9e847da4 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -23,8 +23,9 @@ #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11); // encoder instance Encoder encoder = Encoder(A0, A1, 8192); @@ -48,9 +49,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 7043be25..900f9501 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -1,17 +1,22 @@ // Open loop motor control example #include -// motor instance -// BLDCMotor( phA, phB, phC, pp, (en optional)) -BLDCMotor motor = BLDCMotor(9, 5, 6, 1, 8); -// StepperMotor(ph1A,ph1B,ph2A,ph2B,pp,( en1, en2 optional)) -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // limiting motor movements motor.voltage_limit = 3; // rad/s diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 63340d73..9e8085dd 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -1,18 +1,22 @@ // Open loop motor control example #include -// motor instance -// BLDCMotor( phA, phB, phC, pp, (en optional)) -BLDCMotor motor = BLDCMotor(3, 10, 6, 11, 7); -// StepperMotor(ph1A,ph1B,ph2A,ph2B,pp,( en1, en2 optional)) -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // limiting motor movements motor.voltage_limit = 3; // rad/s diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 6e38917c..b237b693 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -28,10 +28,12 @@ #include #include -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // encoder instance Encoder encoder = Encoder(2, 3, 8192, A0); @@ -55,8 +57,13 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 526b6af2..d5be41b5 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -21,8 +21,12 @@ #include #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // hall sensor instance HallSensor sensor = HallSensor(2, 3, 4, 11); @@ -46,8 +50,14 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index bb110f65..df6ff025 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -16,10 +16,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { @@ -28,9 +30,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino index df8ce490..94749e3c 100644 --- a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino @@ -9,10 +9,13 @@ */ #include -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 7); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // encoder instance Encoder encoder = Encoder(2, 3, 8192); @@ -30,12 +33,16 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init() + // link driver + motor.linkDriver(&driver); + + // aligning voltage motor.voltage_sensor_align = 5; - // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino index 9219bd13..4dc3ac2c 100644 --- a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino @@ -9,8 +9,13 @@ */ #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 7); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // hall sensor instance HallSensor sensor = HallSensor(2, 3, 4, 11); @@ -29,9 +34,13 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init() + // link driver + motor.linkDriver(&driver); + // aligning voltage motor.voltage_sensor_align = 3; diff --git a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino index d9a2d2be..b0d83c80 100644 --- a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -9,16 +9,18 @@ #include // magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); +// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - I2C -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); -// BDLC Motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { @@ -28,14 +30,14 @@ void setup() { motor.linkSensor(&sensor); // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage motor.voltage_sensor_align = 5; - // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - // set motion control loop to be used motor.controller = ControlType::voltage; diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index 18523ac6..50bde4b7 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -29,10 +29,12 @@ #include #include -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // encoder instance Encoder encoder = Encoder(2, 3, 8192, A0); @@ -56,8 +58,13 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 586ef9ab..cb9b9a3d 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -21,8 +21,12 @@ #include #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // hall sensor instance HallSensor sensor = HallSensor(2, 3, 4, 11); @@ -46,11 +50,16 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // aligning voltage [V] motor.voltage_sensor_align = 3; - + // set motion control loop to be used motor.controller = ControlType::velocity; diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 3caf282a..b8e41f8a 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -19,10 +19,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { @@ -31,9 +33,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); // set motion control loop to be used motor.controller = ControlType::velocity; diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 0c4825be..949b9934 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -38,10 +38,12 @@ */ #include -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // encoder instance Encoder encoder = Encoder(2, 3, 8192); @@ -59,12 +61,16 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); - // choose FOC modulation - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init() + // link driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = ControlType::voltage; diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index fed37293..1c4c777b 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -41,8 +41,12 @@ #include #include -// motor instance -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 7); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // hall sensor instance HallSensor sensor = HallSensor(2, 3, 4, 11); @@ -62,19 +66,21 @@ void setup() { sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); - // link the motor to the sensor motor.linkSensor(&sensor); - // choose FOC modulation - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + // driver config // power supply voltage [V] - motor.voltage_power_supply = 12; + driver.voltage_power_supply = 12; + driver.init() + // link driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = ControlType::voltage; - // contoller configuration based on the controll type motor.PID_velocity.P = 0.2; motor.PID_velocity.I = 20; diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 73434cfb..b5ad7e90 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -45,10 +45,13 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); // magnetic sensor instance - analog output // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); -// BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); -// Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { @@ -57,12 +60,16 @@ void setup() { // link the motor to the sensor motor.linkSensor(&sensor); + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init() + // link driver + motor.linkDriver(&driver); + // choose FOC modulation motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - // power supply voltage [V] - motor.voltage_power_supply = 12; - // set control loop type to be used motor.controller = ControlType::voltage; diff --git a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino index 77d1995f..bde79b93 100644 --- a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -1,8 +1,10 @@ #include #include -BLDCMotor motor = BLDCMotor(9, 10, 11, 7); -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +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); @@ -78,7 +80,11 @@ void setup() { Serial.begin(115200); while (!Serial) ; - motor.voltage_power_supply = 9; + // driver config + driver.voltage_power_supply = 12; + driver.init() + motor.linkDriver(&driver); + motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; diff --git a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index 0442036d..167b2e86 100644 --- a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -15,11 +15,14 @@ */ #include -// BLDCMotor( phA, phB, phC, pp, (en optional)) +// BLDC motor instance // its important to put pole pairs number as 1!!! -BLDCMotor motor = BLDCMotor(9, 5, 6, 1, 8); -// StepperMotor(ph1A,ph1B,ph2A,ph2B,pp,( en1, en2 optional)) -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 1, 8); +BLDCMotor motor = BLDCMotor(1); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor instance +// its important to put pole pairs number as 1!!! +//StepperMotor motor = StepperMotor(1); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // Encoder(int encA, int encB , int cpr, int index) Encoder encoder = Encoder(2, 3, 2048); @@ -37,12 +40,14 @@ void setup() { motor.linkSensor(&encoder); // power supply voltage - motor.voltage_power_supply = 12; + // default 12V + driver.voltage_power_supply = 12; + driver.init() + motor.linkDriver(&driver); + // initialize motor motor.init(); - - // monitoring port Serial.begin(115200); diff --git a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index f8355506..0a86f82d 100644 --- a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -15,11 +15,20 @@ */ #include -// BLDCMotor( phA, phB, phC, pp, (en optional)) +// BLDC motor instance // its important to put pole pairs number as 1!!! -BLDCMotor motor = BLDCMotor(9, 5, 6, 1, 8); -// StepperMotor(ph1A,ph1B,ph2A,ph2B,pp,( en1, en2 optional)) -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 1, 8); +BLDCMotor motor = BLDCMotor(1); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor instance +// its important to put pole pairs number as 1!!! +//StepperMotor motor = StepperMotor(1); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// Encoder(int encA, int encB , int cpr, int index) +Encoder encoder = Encoder(2, 3, 2048); +// interrupt routine intialisation +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); @@ -36,7 +45,10 @@ void setup() { motor.linkSensor(&sensor); // power supply voltage - motor.voltage_power_supply = 12; + // default 12V + driver.voltage_power_supply = 12; + driver.init() + motor.linkDriver(&driver); // initialize motor hardware motor.init(); diff --git a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 86d69f6b..582ddbc8 100644 --- a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -20,20 +20,24 @@ MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); // BLDC motor instance -BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8); +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // Stepper motor instance -//StepperMotor motor = StepperMotor(9, 5, 10, 6, 50, 8); +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { + // power supply voltage + driver.voltage_power_supply = 12; + driver.init() + motor.linkDriver(&driver); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor motor.linkSensor(&sensor); - // power supply voltage - // default 12V - motor.voltage_power_supply = 12; // aligning voltage motor.voltage_sensor_align = 7; @@ -42,7 +46,6 @@ void setup() { // initialize motor motor.init(); - // align sensor and start FOC motor.initFOC(); diff --git a/keywords.txt b/keywords.txt index a1eee83e..77966b94 100644 --- a/keywords.txt +++ b/keywords.txt @@ -9,6 +9,7 @@ MagneticSensorSPI KEYWORD1 MagneticSensorI2C KEYWORD1 MagneticSensorAnalog KEYWORD1 BLDCDriver3PWM KEYWORD1 +BLDCDriver6PWM KEYWORD1 BLDCDriver KEYWORD1 StepperDriver4PWM KEYWORD1 StepperDriver KEYWORD1 @@ -25,6 +26,7 @@ _setPwmFrequency KEYWORD3 _writeDutyCycle KEYWORD3 _round KEYWORD3 _sign KEYWORD3 +_constrain KEYWORD3 monitor KEYWORD3 command KEYWORD3 @@ -70,6 +72,7 @@ quadrature KEYWORD2 foc_modulation KEYWORD2 target KEYWORD2 pwm_frequency KEYWORD2 +dead_zone KEYWORD2 voltage KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index cc248dce..a6096857 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,13 +1,13 @@ #include "BLDCMotor.h" -// BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - cpr - counts per rotation number (cpm=ppm*4) -// - enable pin - (optional input) -BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ - -} +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } // BLDCMotor( int pp) // - phA, phB, phC - motor A,B,C phase pwm pins diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 48e48f62..4ad76d89 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -1,17 +1,12 @@ -/** - * @file BLDCMotor.h - * - */ - #ifndef BLDCMotor_h #define BLDCMotor_h #include "Arduino.h" -#include "common/interfaces/FOCMotor.h" -#include "common/interfaces/Sensor.h" -#include "common/interfaces/BLDCDriver.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" #include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "common/time_utils.h" #include "common/defaults.h" /** @@ -26,7 +21,6 @@ class BLDCMotor: public FOCMotor */ BLDCMotor(int pp); - /** * Function linking a motor and a foc driver * diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 9d1e9225..5d93273a 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -88,12 +88,13 @@ void loop() { #include "BLDCMotor.h" #include "StepperMotor.h" -#include "Encoder.h" -#include "MagneticSensorSPI.h" -#include "MagneticSensorI2C.h" -#include "MagneticSensorAnalog.h" -#include "HallSensor.h" -#include "BLDCDriver3PWM.h" -#include "StepperDriver4PWM.h" +#include "sensors/Encoder.h" +#include "sensors/MagneticSensorSPI.h" +#include "sensors/MagneticSensorI2C.h" +#include "sensors/MagneticSensorAnalog.h" +#include "sensors/HallSensor.h" +#include "drivers/BLDCDriver3PWM.h" +#include "drivers/BLDCDriver6PWM.h" +#include "drivers/StepperDriver4PWM.h" #endif diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 70418a6b..7786e4d7 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -7,11 +7,11 @@ #define StepperMotor_h #include "Arduino.h" -#include "common/interfaces/FOCMotor.h" -#include "common/interfaces/StepperDriver.h" -#include "common/interfaces/Sensor.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" #include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "common/time_utils.h" #include "common/defaults.h" /** @@ -25,8 +25,6 @@ class StepperMotor: public FOCMotor @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) */ StepperMotor(int pp); - - /** * Function linking a motor and a foc driver diff --git a/src/common/interfaces/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h similarity index 96% rename from src/common/interfaces/BLDCDriver.h rename to src/common/base_classes/BLDCDriver.h index f53488d2..708323fb 100644 --- a/src/common/interfaces/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,7 +5,7 @@ class BLDCDriver{ public: /** Initialise hardware */ - virtual void init(); + virtual int init(); /** Enable hardware */ virtual void enable(); /** Disable hardware */ diff --git a/src/common/interfaces/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp similarity index 100% rename from src/common/interfaces/FOCMotor.cpp rename to src/common/base_classes/FOCMotor.cpp diff --git a/src/common/interfaces/FOCMotor.h b/src/common/base_classes/FOCMotor.h similarity index 99% rename from src/common/interfaces/FOCMotor.h rename to src/common/base_classes/FOCMotor.h index 4283e2c9..e43dd039 100644 --- a/src/common/interfaces/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -5,7 +5,7 @@ #include "Sensor.h" #include "BLDCDriver.h" -#include "../hardware_utils.h" +#include "../time_utils.h" #include "../foc_utils.h" #include "../defaults.h" #include "../pid.h" diff --git a/src/common/interfaces/Sensor.h b/src/common/base_classes/Sensor.h similarity index 100% rename from src/common/interfaces/Sensor.h rename to src/common/base_classes/Sensor.h diff --git a/src/common/interfaces/StepperDriver.h b/src/common/base_classes/StepperDriver.h similarity index 95% rename from src/common/interfaces/StepperDriver.h rename to src/common/base_classes/StepperDriver.h index 17ea5c55..7800a4e7 100644 --- a/src/common/interfaces/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -5,7 +5,7 @@ class StepperDriver{ public: /** Initialise hardware */ - virtual void init(); + virtual int init(); /** Enable hardware */ virtual void enable(); /** Disable hardware */ diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 155528b5..7da3f7bc 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -21,7 +21,6 @@ #define _2PI 6.28318530718 #define _3PI_2 4.71238898038 - #define NOT_SET -12345.0 /** diff --git a/src/common/hardware_specific/arduino_specific.cpp b/src/common/hardware_specific/arduino_specific.cpp deleted file mode 100644 index 31cfec1c..00000000 --- a/src/common/hardware_specific/arduino_specific.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "../hardware_utils.h" - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) - -// set pwm frequency to 32KHz -void _pinHighFrequency(const int pin){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips - // High PWM frequency - // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - } - if (pin == 9 || pin == 10 ) - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) - TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - -#endif -} - - -// function setting the high pwm frequency to the supplied pins -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); - _pinHighFrequency(pinC); - if(pinD != NOT_SET) _pinHighFrequency(pinD); // stepper motor -#endif -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); -} - - -// function buffering delay() -// arduino uno function doesn't work well with interrupts -void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) - // if arduino uno and other atmega328p chips - // use while instad of delay, - // due to wrong measurement based on changed timer0 - unsigned long t = _micros() + ms*1000; - while( _micros() < t ){}; -#else - // regular micros - delay(ms); -#endif -} - - -// function buffering _micros() -// arduino function doesn't work well with interrupts -unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) -// if arduino uno and other atmega328p chips - //return the value based on the prescaler - if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); - else return (micros()); -#else - // regular micros - return micros(); -#endif -} - -#endif \ No newline at end of file diff --git a/src/common/hardware_specific/arm_specific.cpp b/src/common/hardware_specific/arm_specific.cpp deleted file mode 100644 index 8753d980..00000000 --- a/src/common/hardware_specific/arm_specific.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include "../hardware_utils.h" - -#if defined(__arm__) && defined(CORE_TEENSY) - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin){ - analogWrite(pin, 0); - analogWriteFrequency(pin, freq); -} - - -// function setting the high pwm frequency to the supplied pins -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); -} - - -// function buffering delay() -// arduino uno function doesn't work well with interrupts -void _delay(unsigned long ms){ - // regular micros - delay(ms); -} - - -// function buffering _micros() -// arduino function doesn't work well with interrupts -unsigned long _micros(){ - // regular micros - return micros(); -} - - -#endif \ No newline at end of file diff --git a/src/common/hardware_specific/stm32_specific.cpp b/src/common/hardware_specific/stm32_specific.cpp deleted file mode 100644 index 41fbf764..00000000 --- a/src/common/hardware_specific/stm32_specific.cpp +++ /dev/null @@ -1,66 +0,0 @@ - -#include "../hardware_utils.h" - -#if defined(_STM32_DEF_) - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin){ - analogWrite(pin, 0); - analogWriteFrequency(freq); - analogWriteResolution(12); // resolution 12 bit 0 - 4096 -} - - -// function setting the high pwm frequency to the supplied pins -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,4095] - analogWrite(pinA, 4095.0*dc_a); - analogWrite(pinB, 4095.0*dc_b); - analogWrite(pinC, 4095.0*dc_c); -} - -// function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,4095] - analogWrite(pin1A, 4095.0*dc_1a); - analogWrite(pin1B, 4095.0*dc_1b); - analogWrite(pin2A, 4095.0*dc_2a); - analogWrite(pin2B, 4095.0*dc_2b); -} - - -// function buffering delay() -// arduino uno function doesn't work well with interrupts -void _delay(unsigned long ms){ - // regular micros - delay(ms); -} - - -// function buffering _micros() -// arduino function doesn't work well with interrupts -unsigned long _micros(){ - // regular micros - return micros(); -} -#endif \ No newline at end of file diff --git a/src/common/hardware_utils.h b/src/common/hardware_utils.h deleted file mode 100644 index 3b70af1a..00000000 --- a/src/common/hardware_utils.h +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H - -#include "foc_utils.h" - -/** - * High PWM frequency setting function - * - hardware specific - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pinA pinA bldc motor or pin1A stepper motor - * @param pinB pinB bldc motor or pin1B stepper motor - * @param pinC pinC bldc motor or pin2A stepper motor - * @param pinD pin2B stepper motor - */ -void _setPwmFrequency(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD = NOT_SET); - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * 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 _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * 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 _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); - -/** - * Function implementing delay() function in milliseconds - * - blocking function - * - hardware specific - - * @param ms number of milliseconds to wait - */ -void _delay(unsigned long ms); - -/** - * Function implementing timestamp getting function in microseconds - * hardware specific - */ -unsigned long _micros(); - - -#endif \ No newline at end of file diff --git a/src/common/lowpass_filter.h b/src/common/lowpass_filter.h index 5a7619cf..7c51be54 100644 --- a/src/common/lowpass_filter.h +++ b/src/common/lowpass_filter.h @@ -2,7 +2,7 @@ #define LOWPASS_FILTER_H -#include "hardware_utils.h" +#include "time_utils.h" #include "foc_utils.h" diff --git a/src/common/pid.h b/src/common/pid.h index 7ee337c4..0e9ddf54 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -2,7 +2,7 @@ #define PID_H -#include "hardware_utils.h" +#include "time_utils.h" #include "foc_utils.h" /** diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/src/common/time_utils.h b/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/src/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp similarity index 90% rename from src/BLDCDriver3PWM.cpp rename to src/drivers/BLDCDriver3PWM.cpp index 9c0e27f7..bacb4ec5 100644 --- a/src/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -11,7 +11,7 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; - voltage_limit = NOT_SET + voltage_limit = NOT_SET; } @@ -34,7 +34,7 @@ void BLDCDriver3PWM::disable() } // init hardware pins -void BLDCDriver3PWM::init() { +int BLDCDriver3PWM::init() { // PWM pins pinMode(pwmA, OUTPUT); @@ -48,7 +48,8 @@ void BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _setPwmFrequency(pwm_frequency, pwmA, pwmB, pwmC); + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; } @@ -65,5 +66,5 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); } \ No newline at end of file diff --git a/src/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h similarity index 83% rename from src/BLDCDriver3PWM.h rename to src/drivers/BLDCDriver3PWM.h index b05b9852..b6f7d7f8 100644 --- a/src/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -1,10 +1,11 @@ #ifndef BLDCDriver3PWM_h #define BLDCDriver3PWM_h -#include "common/interfaces/BLDCDriver.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" -#include "common/defaults.h" +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" /** 3 pwm bldc driver class @@ -22,7 +23,7 @@ class BLDCDriver3PWM: public BLDCDriver BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp new file mode 100644 index 00000000..a973fd33 --- /dev/null +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -0,0 +1,78 @@ +#include "BLDCDriver6PWM.h" + +BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en){ + // Pin initialization + pwmA_h = phA_h; + pwmB_h = phB_h; + pwmC_h = phC_h; + pwmA_l = phA_l; + pwmB_l = phB_l; + pwmC_l = phC_l; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + + // dead zone initial - 2% + dead_zone = 0.02; + +} + +// enable motor driver +void BLDCDriver6PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0, 0, 0); +} + +// disable motor driver +void BLDCDriver6PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver6PWM::init() { + + // PWM pins + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_h, OUTPUT); + pinMode(pwmC_h, OUTPUT); + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_l, OUTPUT); + pinMode(pwmC_l, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // configure 6pwm + // hardware specific function - depending on driver and mcu + return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} + + +// Set voltage to the pwm pin +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, -voltage_limit, voltage_limit); + Ub = _constrain(Ub, -voltage_limit, voltage_limit); + Uc = _constrain(Uc, -voltage_limit, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} \ No newline at end of file diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h new file mode 100644 index 00000000..d13b69fd --- /dev/null +++ b/src/drivers/BLDCDriver6PWM.h @@ -0,0 +1,57 @@ +#ifndef BLDCDriver6PWM_h +#define BLDCDriver6PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 6 pwm bldc driver class +*/ +class BLDCDriver6PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA_h A phase pwm pin + @param phA_l A phase pwm pin + @param phB_h B phase pwm pin + @param phB_l A phase pwm pin + @param phC_h C phase pwm pin + @param phC_l A phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA_h,pwmA_l; //!< phase A pwm pin number + int pwmB_h,pwmB_l; //!< phase B pwm pin number + int pwmC_h,pwmC_l; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/src/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp similarity index 91% rename from src/StepperDriver4PWM.cpp rename to src/drivers/StepperDriver4PWM.cpp index 379d9c2d..e661ed43 100644 --- a/src/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -38,7 +38,7 @@ void StepperDriver4PWM::disable() } // init hardware pins -void StepperDriver4PWM::init() { +int StepperDriver4PWM::init() { // PWM pins pinMode(pwm1A, OUTPUT); @@ -48,13 +48,13 @@ void StepperDriver4PWM::init() { if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); - // sanity check for the voltage limit configuration if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _setPwmFrequency(pwm_frequency, pwm1A, pwm2A, pwm1B, pwm2B); + _configure4PWM(pwm_frequency, pwm1A, pwm2A, pwm1B, pwm2B); + return 0; } @@ -75,5 +75,5 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { else duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0,1); // write to hardware - _writeDutyCycle(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); } \ No newline at end of file diff --git a/src/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h similarity index 86% rename from src/StepperDriver4PWM.h rename to src/drivers/StepperDriver4PWM.h index 5711bb2c..e4b2ee42 100644 --- a/src/StepperDriver4PWM.h +++ b/src/drivers/StepperDriver4PWM.h @@ -1,10 +1,11 @@ #ifndef STEPPER_DRIVER_4PWM_h #define STEPPER_DRIVER_4PWM_h -#include "common/interfaces/StepperDriver.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" -#include "common/defaults.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" /** 4 pwm stepper driver class @@ -24,7 +25,7 @@ class StepperDriver4PWM: public StepperDriver StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET); /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h new file mode 100644 index 00000000..da5e4d20 --- /dev/null +++ b/src/drivers/hardware_api.h @@ -0,0 +1,99 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @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); + +/** + * 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); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @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); + +/** + * 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); + +/** + * 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); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM 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 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); + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/arm_mcu.cpp b/src/drivers/hardware_specific/arm_mcu.cpp new file mode 100644 index 00000000..10bd24c5 --- /dev/null +++ b/src/drivers/hardware_specific/arm_mcu.cpp @@ -0,0 +1,71 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(pin, freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..f5374ebe --- /dev/null +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,133 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configurePair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configurePair(pinA_h, pinA_l); + ret_flag += _configurePair(pinB_h, pinB_l); + ret_flag += _configurePair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/src/common/hardware_specific/esp32_specific.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp similarity index 65% rename from src/common/hardware_specific/esp32_specific.cpp rename to src/drivers/hardware_specific/esp32_mcu.cpp index f040bd9f..d377b0d2 100644 --- a/src/common/hardware_specific/esp32_specific.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_utils.h" +#include "../hardware_api.h" #if defined(ESP_H) @@ -94,63 +94,71 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 -void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency - if(pinD == NOT_SET){ - bldc_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - for(int i = 0; i < 4; i++){ - if(esp32_bldc_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_motor_slots[i].pinA = pinA; - m_slot = esp32_bldc_motor_slots[i]; - break; - } + bldc_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int i = 0; i < 4; i++){ + if(esp32_bldc_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_motor_slots[i].pinA = pinA; + m_slot = esp32_bldc_motor_slots[i]; + break; } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - }else{ - stepper_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - for(int i = 0; i < 2; i++){ - if(esp32_stepper_motor_slots[i].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_motor_slots[i].pin1A = pinA; - m_slot = esp32_stepper_motor_slots[i]; - break; - } + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + stepper_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int i = 0; i < 2; i++){ + if(esp32_stepper_motor_slots[i].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_motor_slots[i].pin1A = pinA; + m_slot = esp32_stepper_motor_slots[i]; + break; } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); } + // function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +// - BLDC motor - 3PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_bldc_motor_slots[i].pinA == pinA){ // if motor slot found @@ -165,11 +173,10 @@ void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, in } // function setting the pwm duty cycle to the hardware -//- hardware speciffic -// -// Arduino and STM32 devices use analogWrite() -// ESP32 uses MCPWM -void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +// - Stepper motor - 4PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found @@ -184,21 +191,17 @@ void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pi } } - -// function buffering delay() -// arduino uno function doesn't work well with interrupts -void _delay(unsigned long ms){ - // regular micros - delay(ms); +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; } - -// function buffering _micros() -// arduino function doesn't work well with interrupts -unsigned long _micros(){ - // regular micros - return micros(); +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; } - - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp new file mode 100644 index 00000000..dc322746 --- /dev/null +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -0,0 +1,68 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 + +#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy + +#elif defined(ESP_H) // or esp32 + +#elif defined(_STM32_DEF_) // or stm32 + +#else + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + return; +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + return; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..4fa8e0c9 --- /dev/null +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,271 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_FREQUENCY 50000 // 50khz + + +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +// setting pwm to hardware pin - instead analogWrite() +void _setPwm(int ulPin, uint32_t value, int resolution) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +{ + return _initPinPWM(PWM_freq, ulPin); +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + TIM_OC_InitTypeDef sConfigOC = {0}; + sConfigOC.OCMode = TIM_OCMODE_PWM2; + sConfigOC.Pulse = 100; + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, int dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +{ + PinName uhPinName = digitalPinToPinName(pinA_h); + PinName ulPinName = digitalPinToPinName(pinA_l); + PinName vhPinName = digitalPinToPinName(pinB_h); + PinName vlPinName = digitalPinToPinName(pinB_l); + PinName whPinName = digitalPinToPinName(pinC_h); + PinName wlPinName = digitalPinToPinName(pinC_l); + + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, 100); // deadtime is non linear! + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + + HT->pause(); + HT->refresh(); + HT->resume(); + return HT; +} + + +// returns 0 if each pair of pwm channels has same channel +// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns -1 if neither - avoid configuring - error!!! +int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + PinName nameAH = digitalPinToPinName(pinA_h); + PinName nameAL = digitalPinToPinName(pinA_l); + PinName nameBH = digitalPinToPinName(pinB_h); + PinName nameBL = digitalPinToPinName(pinB_l); + PinName nameCH = digitalPinToPinName(pinC_h); + PinName nameCL = digitalPinToPinName(pinC_l); + int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); + int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); + int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); + int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); + int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); + int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +} + + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3, HT4); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + + if(config == _HARDWARE_6PWM){ + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + }else if(config == _SOFTWARE_6PWM){ + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + }else{ + return -1; + } + return 0; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwm(pinA_l, _constrain(dc_a + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} +#endif \ No newline at end of file diff --git a/src/Encoder.cpp b/src/sensors/Encoder.cpp similarity index 100% rename from src/Encoder.cpp rename to src/sensors/Encoder.cpp diff --git a/src/Encoder.h b/src/sensors/Encoder.h similarity index 96% rename from src/Encoder.h rename to src/sensors/Encoder.h index 1e9ca65b..29122888 100644 --- a/src/Encoder.h +++ b/src/sensors/Encoder.h @@ -2,9 +2,9 @@ #define ENCODER_LIB_H #include "Arduino.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" -#include "common/interfaces/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" /** diff --git a/src/HallSensor.cpp b/src/sensors/HallSensor.cpp similarity index 100% rename from src/HallSensor.cpp rename to src/sensors/HallSensor.cpp diff --git a/src/HallSensor.h b/src/sensors/HallSensor.h similarity index 96% rename from src/HallSensor.h rename to src/sensors/HallSensor.h index 9f7226b6..7eb6d1fd 100644 --- a/src/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -2,9 +2,9 @@ #define HALL_SENSOR_LIB_H #include "Arduino.h" -#include "common/interfaces/Sensor.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" class HallSensor: public Sensor{ diff --git a/src/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp similarity index 100% rename from src/MagneticSensorAnalog.cpp rename to src/sensors/MagneticSensorAnalog.cpp diff --git a/src/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h similarity index 95% rename from src/MagneticSensorAnalog.h rename to src/sensors/MagneticSensorAnalog.h index e2938dea..d76b6eb4 100644 --- a/src/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -2,9 +2,9 @@ #define MAGNETICSENSORANALOG_LIB_H #include "Arduino.h" -#include "common/interfaces/Sensor.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" /** * This sensor has been tested with AS5600 running in 'analog mode'. This is where output pin of AS6000 is connected to an analog pin on your microcontroller. diff --git a/src/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp similarity index 100% rename from src/MagneticSensorI2C.cpp rename to src/sensors/MagneticSensorI2C.cpp diff --git a/src/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h similarity index 96% rename from src/MagneticSensorI2C.h rename to src/sensors/MagneticSensorI2C.h index ade14fff..7935aae9 100644 --- a/src/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -3,9 +3,9 @@ #include "Arduino.h" #include -#include "common/interfaces/Sensor.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" struct MagneticSensorI2CConfig_s { int chip_address; diff --git a/src/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp similarity index 100% rename from src/MagneticSensorSPI.cpp rename to src/sensors/MagneticSensorSPI.cpp diff --git a/src/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h similarity index 96% rename from src/MagneticSensorSPI.h rename to src/sensors/MagneticSensorSPI.h index 2212b57f..411b9aef 100644 --- a/src/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -3,9 +3,9 @@ #include "Arduino.h" #include -#include "common/interfaces/Sensor.h" -#include "common/foc_utils.h" -#include "common/hardware_utils.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" #define DEF_ANGLE_REGISTAR 0x3FFF From 63d65caea3337ccdda049ace425d6e4574d51b75 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 13 Nov 2020 17:45:46 +0100 Subject: [PATCH 012/749] TYPO ; missing --- .../encoder/full_control_serial/full_control_serial.ino | 2 +- .../hall_sensor/full_control_serial/full_control_serial.ino | 2 +- .../magnetic_sensor/full_control_serial/full_control_serial.ino | 2 +- .../alignment_and_cogging_test/alignment_and_cogging_test.ino | 2 +- .../encoder/find_pole_pairs_number/find_pole_pairs_number.ino | 2 +- .../find_pole_pairs_number/find_pole_pairs_number.ino | 2 +- .../find_sensor_offset_and_direction.ino | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 949b9934..5ed5ea16 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -64,7 +64,7 @@ void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; - driver.init() + driver.init(); // link driver motor.linkDriver(&driver); diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 1c4c777b..ddbcfe91 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -72,7 +72,7 @@ void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; - driver.init() + driver.init(); // link driver motor.linkDriver(&driver); diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index b5ad7e90..666fe7d0 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -63,7 +63,7 @@ void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; - driver.init() + driver.init(); // link driver motor.linkDriver(&driver); diff --git a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino index bde79b93..a8bd27a7 100644 --- a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -82,7 +82,7 @@ void setup() { // driver config driver.voltage_power_supply = 12; - driver.init() + driver.init(); motor.linkDriver(&driver); motor.voltage_sensor_align = 3; diff --git a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index 167b2e86..b3c26556 100644 --- a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -42,7 +42,7 @@ void setup() { // power supply voltage // default 12V driver.voltage_power_supply = 12; - driver.init() + driver.init(); motor.linkDriver(&driver); diff --git a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index 0a86f82d..5fcf27bd 100644 --- a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -47,7 +47,7 @@ void setup() { // power supply voltage // default 12V driver.voltage_power_supply = 12; - driver.init() + driver.init(); motor.linkDriver(&driver); // initialize motor hardware diff --git a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 582ddbc8..54eb1d17 100644 --- a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -30,7 +30,7 @@ void setup() { // power supply voltage driver.voltage_power_supply = 12; - driver.init() + driver.init(); motor.linkDriver(&driver); // initialise magnetic sensor hardware From 5ce4b48d5d815bef04a7ea227f4bf136ebd2474b Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 13 Nov 2020 17:48:05 +0100 Subject: [PATCH 013/749] TYPO still ; missing --- .../encoder/voltage_control/voltage_control.ino | 2 +- .../hall_sensor/voltage_control/voltage_control.ino | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino index 94749e3c..8d904621 100644 --- a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino @@ -36,7 +36,7 @@ void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; - driver.init() + driver.init(); // link driver motor.linkDriver(&driver); diff --git a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino index 4dc3ac2c..0e942382 100644 --- a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino @@ -37,7 +37,7 @@ void setup() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; - driver.init() + driver.init(); // link driver motor.linkDriver(&driver); From cf61686dbab95539f449c675b6ca15c1c03f2697 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 13 Nov 2020 17:59:12 +0100 Subject: [PATCH 014/749] DOCS readme update --- README.md | 39 +++++++++++++++++++++++++++++---------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 3403b94c..1d4a8247 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,25 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -

NEW RELEASE 📢: SimpleFOClibrary v1.6.0

  • Stepper motor FOC support 🎨🎉 🎊 See in docs!
    • No losing steps
    • Backdrivable
    • Better dynamics than open-loop, Smoother than open-loop
    • short demo youtube video
  • Teensy support by Christopher Parrott
  • Pull requests by @cousinitt
    • refactoring and c++11 improvements
    • pid + low pass filter refactoring
  • Extended configurability of the sensor classes by @owennewo See in docs!
  • configurable pwm frequency See in docs!
    • stm32,teensy,eps32 - not for Arduino
    • stm32 added 12bit pwm resolution by Jürgen Frisch
  • Huge refactoring done in the library 😄
+

NEW RELEASE 📢: SimpleFOClibrary v1.7.0

    +
  • 6PWM support +
      +
    • Arduino UNO
      • +
    • stm32 boards
      • +
    +
    • +
  • BLDC driver code separated +
      +
    • BLDC: 6pwm and 3pwm
      • +
    • Stepper: 4pwm
      • +
    • Hardware specific code in separate files
      • +
    • PWM config
      • +
    +
    • +
  • Refactoring
    • +
+The library version 1.7.0 will be released once when it is properly tested and documented! +
## Arduino *SimpleFOCShield* @@ -121,8 +139,10 @@ NOTE: This program uses all the default control parameters. ```cpp #include -// BLDCMotor( pin_pwmA, pin_pwmB, pin_pwmC, pole_pairs, enable (optional)) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); +// BLDCMotor( pole_pairs ) +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) +BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); // Encoder(pin_A, pin_B, CPR) Encoder encoder = Encoder(2, 3, 2048); // channel A and B callbacks @@ -138,10 +158,12 @@ void setup() { // link the motor to the sensor motor.linkSensor(&encoder); - // use monitoring with the BLDCMotor - Serial.begin(115200); - // monitoring port - motor.useMonitoring(Serial); + // power supply voltage [V] + driver.voltage_power_supply = 12; + // initialise driver hardware + driver.init(); + // link driver + motor.linkDriver(&driver); // set control loop type to be used motor.controller = ControlType::velocity; @@ -159,9 +181,6 @@ void loop() { // velocity control loop function // setting the target velocity or 2rad/s motor.move(2); - - // monitoring function outputting motor variables to the serial terminal - motor.monitor(); } ``` You can find more details in the [SimpleFOC documentation](https://docs.simplefoc.com/). From 47a51e8ab93c7113e103e64c2fa668e49c7acff9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 18:34:31 +0100 Subject: [PATCH 015/749] FEAT eps32 6pwm implementation --- README.md | 1 + src/drivers/hardware_api.h | 1 + src/drivers/hardware_specific/esp32_mcu.cpp | 151 +++++++++++++++--- .../{arm_mcu.cpp => teensy_mcu.cpp} | 0 4 files changed, 132 insertions(+), 21 deletions(-) rename src/drivers/hardware_specific/{arm_mcu.cpp => teensy_mcu.cpp} (100%) diff --git a/README.md b/README.md index 1d4a8247..5d4ccfd4 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,7 @@ Therefore this is an attempt to:
  • Arduino UNO
  • stm32 boards
    • +
  • esp32 boards
  • BLDC driver code separated diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index da5e4d20..da5ddf51 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -2,6 +2,7 @@ #define HARDWARE_UTILS_H #include "../common/foc_utils.h" +#include "../common/time_utils.h" /** * Configuring PWM frequency, resolution and alignment diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index d377b0d2..5030ac11 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -8,6 +8,7 @@ // empty motor slot #define _EMPTY_SLOT -20 +#define _TAKEN_SLOT -21 // structure containing motor slot configuration // this library supports up to 4 motors @@ -19,7 +20,7 @@ typedef struct { mcpwm_io_signals_t mcpwm_a; mcpwm_io_signals_t mcpwm_b; mcpwm_io_signals_t mcpwm_c; -} bldc_motor_slots_t; +} bldc_3pwm_motor_slots_t; typedef struct { int pin1A; mcpwm_dev_t* mcpwm_num; @@ -32,8 +33,22 @@ typedef struct { mcpwm_io_signals_t mcpwm_2b; } stepper_motor_slots_t; +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + // define bldc motor slots array -bldc_motor_slots_t esp32_bldc_motor_slots[4] = { +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A @@ -42,8 +57,14 @@ bldc_motor_slots_t esp32_bldc_motor_slots[4] = { // define stepper motor slots array stepper_motor_slots_t esp32_stepper_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define stepper motor slots array +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 }; // configuring high frequency pwm timer @@ -102,18 +123,30 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency - bldc_motor_slots_t m_slot = {}; + bldc_3pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters - for(int i = 0; i < 4; i++){ - if(esp32_bldc_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_motor_slots[i].pinA = pinA; - m_slot = esp32_bldc_motor_slots[i]; + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; break; } } - + // disable all the slots with the same MCPWM + if( slot_num < 2 ){ + // slot 0 of the stepper + esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); @@ -135,14 +168,29 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int stepper_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters - for(int i = 0; i < 2; i++){ - if(esp32_stepper_motor_slots[i].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_motor_slots[i].pin1A = pinA; - m_slot = esp32_stepper_motor_slots[i]; + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_motor_slots[slot_num]; break; } } - + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); @@ -153,7 +201,6 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); } - // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic @@ -161,12 +208,12 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ - if(esp32_bldc_motor_slots[i].pinA == pinA){ // if motor slot found + if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,2047] - esp32_bldc_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047; - esp32_bldc_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047; - esp32_bldc_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047; + esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047; + esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047; + esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047; break; } } @@ -195,13 +242,75 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - return -1; + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz 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 + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(!m_slot.mcpwm_unit) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + // return + return 0; } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - return; + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100.0] + float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; + float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; + float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; + float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; + float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; + float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_ah); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_al); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_bh); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_bl); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_ch); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_cl); + break; + } + } } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/arm_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp similarity index 100% rename from src/drivers/hardware_specific/arm_mcu.cpp rename to src/drivers/hardware_specific/teensy_mcu.cpp From 33c5a1975e285c434a98826ed0fe4c28c19d66cf Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:01:20 +0100 Subject: [PATCH 016/749] FIX deaditme --- src/drivers/hardware_specific/esp32_mcu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 5030ac11..f765d531 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -81,6 +81,10 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm 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_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + _delay(100); mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); From 7febff80bf130b01136f6aa88fabc9876a59c57e Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:22:02 +0100 Subject: [PATCH 017/749] FIX fixed config --- src/drivers/hardware_specific/esp32_mcu.cpp | 77 +++++++++++++++------ 1 file changed, 54 insertions(+), 23 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f765d531..7f990ab1 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -64,7 +64,7 @@ stepper_motor_slots_t esp32_stepper_motor_slots[2] = { // define stepper motor slots array bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM1 }; // configuring high frequency pwm timer @@ -81,10 +81,6 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm 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_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - _delay(100); mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); @@ -287,34 +283,69 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + _configureTimerFrequency6PWM(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); // return return 0; } + +// configuring high frequency pwm timer +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency6PWM(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ + + mcpwm_config_t pwm_config; + + pwm_config.frequency = 4000; //frequency = 20000Hz + pwm_config.cmpr_a = 50.0; //duty cycle of PWMxA = 50.0% + pwm_config.cmpr_b = 50.0; //duty cycle of PWMxB = 50.0% + pwm_config.counter_mode = MCPWM_UP_COUNTER; //MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + + MCPWM0.timer[0].sync.out_sel = 1; + delayMicroseconds(1000); + MCPWM0.timer[0].sync.out_sel = 0; +} + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // for(int i = 0; i < 2; i++){ + // if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; - float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; - float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; - float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; - float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; - float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_ah); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_al); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_bh); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_bl); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_ch); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_cl); - break; - } - } + // float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; + // float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; + // float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; + // float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; + // float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; + // float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + // break; + // } + // } } #endif \ No newline at end of file From b009a75db1891777ab228905cf5845c19f59437b Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:27:44 +0100 Subject: [PATCH 018/749] FIX even morte fixing --- src/drivers/hardware_specific/esp32_mcu.cpp | 83 +++++++++++---------- 1 file changed, 44 insertions(+), 39 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 7f990ab1..23b6772f 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -243,45 +243,50 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz 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 - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - break; - } - } - // if no slots available - if(!m_slot.mcpwm_unit) return -1; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - + // if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + // else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz 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 + // int slot_num; + // for(slot_num = 0; slot_num < 2; slot_num++){ + // if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + // esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + // m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + // break; + // } + // } + // // if no slots available + // if(!m_slot.mcpwm_unit) return -1; + + // // disable all the slots with the same MCPWM + // if( slot_num == 0 ){ + // // slots 0 and 1 of the 3pwm bldc + // esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + // esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // // slot 0 of the 6pwm bldc + // esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + // }else{ + // // slots 2 and 3 of the 3pwm bldc + // esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + // esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // // slot 1 of the 6pwm bldc + // esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + // } + + // // configure pins + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, pinA_h); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, pinA_l); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, pinB_h); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, pinB_l); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, pinC_h); + mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, pinC_l); // configure the timer _configureTimerFrequency6PWM(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); // return From 64379c705bb402a5cfb8bd4963a622a6ba3ebd8f Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:32:56 +0100 Subject: [PATCH 019/749] FIX fixed pwm no moving --- src/drivers/hardware_specific/esp32_mcu.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 23b6772f..3d05ffac 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -5,6 +5,7 @@ #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" +#include "wiring_private.h" // pinPeripheral() function // empty motor slot #define _EMPTY_SLOT -20 @@ -343,12 +344,12 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); // break; // } // } From e56b338f7d072b9ab23129ae12779714e9c72045 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:33:24 +0100 Subject: [PATCH 020/749] FIX removed changing pwm --- src/drivers/hardware_specific/esp32_mcu.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 3d05ffac..1172dfa7 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -344,12 +344,12 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); + // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); // break; // } // } From c296d7336ccd2f8c937e41e91a37596707b2c6e6 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 19:34:38 +0100 Subject: [PATCH 021/749] FIX move --- src/drivers/hardware_specific/esp32_mcu.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 1172dfa7..9b4928c7 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -5,7 +5,6 @@ #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -#include "wiring_private.h" // pinPeripheral() function // empty motor slot #define _EMPTY_SLOT -20 @@ -320,6 +319,12 @@ void _configureTimerFrequency6PWM(long pwm_frequency, mcpwm_dev_t* mcpwm_num, m MCPWM0.timer[0].sync.out_sel = 1; delayMicroseconds(1000); MCPWM0.timer[0].sync.out_sel = 0; + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); } From 40445aa83a83b311b9e41473be36255e291b22c5 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 20:07:55 +0100 Subject: [PATCH 022/749] FIX latest --- src/drivers/hardware_specific/esp32_mcu.cpp | 69 +++++++++++---------- 1 file changed, 35 insertions(+), 34 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 9b4928c7..112b70d4 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -238,6 +238,41 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in } } +// configuring high frequency pwm timer +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller/log/184464-mcpwm-with-deadband +void _configureTimerFrequency6PWM(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ + + mcpwm_config_t pwm_config; + pwm_config.frequency = 4000; // frequency = 20000Hz + pwm_config.cmpr_a = 50.0; // duty cycle of PWMxA = 50.0% + pwm_config.cmpr_b = 50.0; // duty cycle of PWMxB = 50.0% + pwm_config.counter_mode = MCPWM_UP_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); // Configure PWM0A & PWM0B with above settings + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); // Configure PWM0A & PWM0B with above settings + mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); // Configure PWM0A & PWM0B with above settings + + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); + + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + + MCPWM0.timer[0].sync.out_sel = 1; + delayMicroseconds(1000); + MCPWM0.timer[0].sync.out_sel = 0; + + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); + mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); +} + + // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific @@ -294,40 +329,6 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } -// configuring high frequency pwm timer -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -void _configureTimerFrequency6PWM(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ - - mcpwm_config_t pwm_config; - - pwm_config.frequency = 4000; //frequency = 20000Hz - pwm_config.cmpr_a = 50.0; //duty cycle of PWMxA = 50.0% - pwm_config.cmpr_b = 50.0; //duty cycle of PWMxB = 50.0% - pwm_config.counter_mode = MCPWM_UP_COUNTER; //MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); - - MCPWM0.timer[0].sync.out_sel = 1; - delayMicroseconds(1000); - MCPWM0.timer[0].sync.out_sel = 0; - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); -} - - // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific From 47ba16748778eff90eb58fd85faf4a69e5f8e165 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 20:11:30 +0100 Subject: [PATCH 023/749] FIX debuggig --- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 112b70d4..2aa8f172 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -323,7 +323,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, pinC_h); mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, pinC_l); // configure the timer - _configureTimerFrequency6PWM(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + _configureTimerFrequency6PWM(pwm_frequency, 0, 0); // return return 0; } From 9f984c280b3ba369becf986a682ec45e1dafc7de Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 14 Nov 2020 20:29:42 +0100 Subject: [PATCH 024/749] Revert "FIX deaditme" This reverts commit 33c5a1975e285c434a98826ed0fe4c28c19d66cf. --- src/drivers/hardware_specific/esp32_mcu.cpp | 163 +++++++------------- 1 file changed, 58 insertions(+), 105 deletions(-) diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 2aa8f172..5030ac11 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -64,7 +64,7 @@ stepper_motor_slots_t esp32_stepper_motor_slots[2] = { // define stepper motor slots array bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM1 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 }; // configuring high frequency pwm timer @@ -238,126 +238,79 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in } } -// configuring high frequency pwm timer -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller/log/184464-mcpwm-with-deadband -void _configureTimerFrequency6PWM(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ - - mcpwm_config_t pwm_config; - pwm_config.frequency = 4000; // frequency = 20000Hz - pwm_config.cmpr_a = 50.0; // duty cycle of PWMxA = 50.0% - pwm_config.cmpr_b = 50.0; // duty cycle of PWMxB = 50.0% - pwm_config.counter_mode = MCPWM_UP_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); // Configure PWM0A & PWM0B with above settings - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); // Configure PWM0A & PWM0B with above settings - mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); // Configure PWM0A & PWM0B with above settings - - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, 40, 40); - - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); - mcpwm_sync_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); - - MCPWM0.timer[0].sync.out_sel = 1; - delayMicroseconds(1000); - MCPWM0.timer[0].sync.out_sel = 0; - - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); - mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); -} - - // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - // if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - // else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz 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 - // int slot_num; - // for(slot_num = 0; slot_num < 2; slot_num++){ - // if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - // esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - // m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - // break; - // } - // } - // // if no slots available - // if(!m_slot.mcpwm_unit) return -1; - - // // disable all the slots with the same MCPWM - // if( slot_num == 0 ){ - // // slots 0 and 1 of the 3pwm bldc - // esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - // esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // // slot 0 of the 6pwm bldc - // esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; - // }else{ - // // slots 2 and 3 of the 3pwm bldc - // esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - // esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // // slot 1 of the 6pwm bldc - // esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; - // } - - // // configure pins - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - // mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, pinA_h); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, pinA_l); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, pinB_h); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, pinB_l); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, pinC_h); - mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, pinC_l); + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz 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 + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(!m_slot.mcpwm_unit) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + // configure the timer - _configureTimerFrequency6PWM(pwm_frequency, 0, 0); + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); // return return 0; } - // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ // determine which motor slot is the motor connected to - // for(int i = 0; i < 2; i++){ - // if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - // float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; - // float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; - // float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; - // float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; - // float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; - // float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - // mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, 50); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, 50); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, 50); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, 50); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, 50); - // mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, 50); - // break; - // } - // } + float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; + float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; + float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; + float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; + float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; + float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_ah); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_al); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_bh); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_bl); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_ch); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_cl); + break; + } + } } #endif \ No newline at end of file From d430f69126149cce02d2049aa477cbdb3cad8371 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 15 Nov 2020 10:47:09 +0100 Subject: [PATCH 025/749] FEAT a bit of docs update --- README.md | 72 +++++++++++++++++++++---------------------------- src/SimpleFOC.h | 54 ++++++++++++++++++++++--------------- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/README.md b/README.md index 5d4ccfd4..c6ca978b 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Arduino Simple Field Oriented Control (FOC) library -![Library Compile](https://github.com/askuric/Arduino-FOC/workflows/Library%20Compile/badge.svg) +![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) @@ -14,7 +14,7 @@ Therefore this is an attempt to:

    NEW RELEASE 📢: SimpleFOClibrary v1.7.0

    • 6PWM support
        -
      • Arduino UNO
        • +
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
      @@ -52,7 +52,7 @@ Therefore this is an attempt to: - **Arduino headers**: Arduino UNO, Arduino MEGA, STM32 Nucleo boards... - **Open Source**: Fully available fabrication files - [how to make it yourself](https://www.simplefoc.com/arduino_simplefoc_shield_fabrication), -##### If you are interested in this board, order your version on this link: [Simple FOC Shop](https://www.simplefoc.com/simplefoc_shield_product) +##### If you are interested in this board, order your version on this link: [Shop](https://www.simplefoc.com/simplefoc_shield_product)

      @@ -65,7 +65,7 @@ Therefore this is an attempt to:

      -This video demonstrates the Simple FOC library basic usage, electronic connections and shows its capabilities. +This video demonstrates the *Simple**FOC**library* basic usage, electronic connections and shows its capabilities. ### Features @@ -91,46 +91,37 @@ This video demonstrates the Simple FOC library basic usage, electronic connectio 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. - Full library installation [Docs](https://docs.simplefoc.com/library_download) -- Minimal code installation [Docs](https://docs.simplefoc.com/minimal_download) +- Minimal project builder [Docs](https://docs.simplefoc.com/minimal_download) -### Arduino SimpleFOC library installation to Arduino IDE +### Arduino *SimpleFOClibrary* installation to Arduino IDE #### Arduino Library Manager The simplest way to get hold of the library is directly by using Arduino IDE and its integrated Library Manager. - Open Arduino IDE and start Arduino Library Manager by clicking: `Tools > Manage Libraries...`. - Search for `Simple FOC` library and install the latest version. - Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`. -### Using Github website -- Go to the [github repository](https://github.com/askuric/Arduino-FOC) +#### Using Github website +- Go to the [github repository](https://github.com/simplefoc/Arduino-FOC) - Click first on `Clone or Download > Download ZIP`. - Unzip it and place it in `Arduino Libraries` folder. Windows: `Documents > Arduino > libraries`. - Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`. -### Using terminal +#### Using terminal - Open terminal and run ```sh -cd *arduino libraries folder* -git clone https://github.com/askuric/Arduino-FOC.git +cd #Arduino libraries folder +git clone https://github.com/simplefoc/Arduino-FOC.git ``` - Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`. -### SimpleFOC library minimal sketch example +### *SimpleFOClibrary* minimal project builder -For those willing to experiment and to modify the code I suggest using the [minimal version](https://github.com/askuric/Arduino-FOC/tree/minimal) of the code. +For those willing to experiment and to modify the code I suggest using the minimal project builder [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal). > This code is completely independent and you can run it as any other Arduino Sketch without the need for any libraries. -#### Github website download -- Go to [minimal branch](https://github.com/askuric/Arduino-FOC/tree/minimal) -- Download the code by clicking on the `Clone or Download > Download ZIP`. -- Unzip it and open the sketch in Arduino IDE. - -#### Using terminal -- Open the terminal: - ```sh - cd *to you desired directory* - git clone -b minimal https://github.com/askuric/Arduino-FOC.git - ``` -- Then you just open it with the Arduino IDE and run it. +All you need to do is: +- Go to [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal) +- Follow the tutorial in the README file and choose only the library files that are necessary for your application. ## Arduino code example This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder. @@ -187,23 +178,22 @@ void loop() { You can find more details in the [SimpleFOC documentation](https://docs.simplefoc.com/). ## Example projects -Here are some of the SimpleFOC application examples. -### Arduino Field Oriented Controlled Reaction Wheel Inverted Pendulum -This is a very cool open-source project of one of the simplest setups of the Reaction wheel inverted pendulum. Check out all the components and projects notes in the [github repository](https://github.com/askuric/Arduino-FOC-reaction-wheel-inverted-pendulum). -

      +Here are some of the *Simple**FOC**library* and *Simple**FOC**Shield* application examples. +

      - + + + + + + + + + +

      -**The main benefits of using the BLDC motor in this project are:** -- High torque to weight ratio - - The lighter the better -- Lots of torque for low angular velocities - - No need to spin the motor to very high PRM to achieve high torques -- No gearboxes and backlash - - Very smooth operation = very stable pendulum - ## Documentation Find out more information about the Arduino SimpleFOC project in [docs website](https://docs.simplefoc.com/) @@ -212,6 +202,6 @@ Find out more information about the Arduino SimpleFOC project in [docs website]( ## Arduino FOC repo structure Branch | Description | Status ------------ | ------------- | ------------ -[master](https://github.com/askuric/Arduino-FOC) | Stable and tested library version | ![Library Compile](https://github.com/askuric/Arduino-FOC/workflows/Library%20Compile/badge.svg) -[dev](https://github.com/askuric/Arduino-FOC/tree/dev) | Development library version | ![Library Dev Compile](https://github.com/askuric/Arduino-FOC/workflows/Library%20Dev%20Compile/badge.svg?branch=dev) -[minimal](https://github.com/askuric/Arduino-FOC/tree/minimal) | Minimal Arduino example with integrated library | ![MinimalBuild](https://github.com/askuric/Arduino-FOC/workflows/MinimalBuild/badge.svg?branch=minimal) +[master](https://github.com/simplefoc/Arduino-FOC) | Stable and tested library version | ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) +[dev](https://github.com/simplefoc/Arduino-FOC/tree/dev) | Development library version | ![Library Dev Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Dev%20Compile/badge.svg?branch=dev) +[minimal](https://github.com/simplefoc/Arduino-FOC/tree/minimal) | Minimal Arduino example with integrated library | ![MinimalBuild](https://github.com/simplefoc/Arduino-FOC/workflows/MinimalBuild/badge.svg?branch=minimal) diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 5d93273a..41188c20 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -20,21 +20,33 @@ * - Plug & play: Arduino SimpleFOC shield * * @section dependencies Supported Hardware - * - * This library supports any arduino device and it is especially optimized for Arduino UNO boards and - * other Atmega328 boards. But it supports Arrduinio MEGA boards and similar. - * - * From the version 1.3.0 it will support the STM32 boards such as Bluepill and Nucelo devices.
      - * The programming is done the same way as for the Arduino UNO but stm32 devices require STM32Duino package.
      - * You can download it directly from library manager. + * - Motors + * - BLDC motors + * - Stepper motors + * - Drivers + * - BLDC drivers + * - Gimbal drivers + * - Stepper drivers + * - Position sensors + * - Encoders + * - Magnetic sensors + * - Hall sensors + * - Open-loop control + * - Microcontrollers + * - Arduino + * - STM32 + * - ESP32 + * - Teensy * * @section example_code Example code * @code #include -// initialize the motor -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// initialize the encoder +// BLDCMotor( pole_pairs ) +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) +BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); +// Encoder(pin_A, pin_B, CPR) Encoder encoder = Encoder(2, 3, 2048); // channel A and B callbacks void doA(){encoder.handleA();} @@ -46,20 +58,21 @@ void setup() { encoder.init(); // hardware interrupt enable encoder.enableInterrupts(doA, doB); - - // set control loop type to be used - motor.controller = ControlType::velocity; - - // use monitoring with the BLDCMotor - Serial.begin(115200); - // monitoring port - motor.useMonitoring(Serial); - // link the motor to the sensor motor.linkSensor(&encoder); + + // power supply voltage [V] + driver.voltage_power_supply = 12; + // initialise driver hardware + driver.init(); + // link driver + motor.linkDriver(&driver); + // set control loop type to be used + motor.controller = ControlType::velocity; // initialize motor motor.init(); + // align encoder and start FOC motor.initFOC(); } @@ -71,9 +84,6 @@ void loop() { // velocity control loop function // setting the target velocity or 2rad/s motor.move(2); - - // monitoring function outputting motor variables to the serial terminal - motor.monitor(); } * @endcode * From e6b8f5bbaf3e010067c06ea4f1bc38d14e81a1c2 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 15 Nov 2020 17:29:44 +0100 Subject: [PATCH 026/749] README update --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c6ca978b..f3f4d9e4 100644 --- a/README.md +++ b/README.md @@ -134,7 +134,7 @@ NOTE: This program uses all the default control parameters. // BLDCMotor( pole_pairs ) BLDCMotor motor = BLDCMotor(11); // BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) -BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // Encoder(pin_A, pin_B, CPR) Encoder encoder = Encoder(2, 3, 2048); // channel A and B callbacks From 929fb29bb7bab76baefd97f80f0967f3c2b9159f Mon Sep 17 00:00:00 2001 From: qazxswedcvfrtgbnhyujmkiolp <2b377c205d@gmail.com> Date: Sun, 15 Nov 2020 23:02:33 -0600 Subject: [PATCH 027/749] fix: steppers not working on esp32 a typo --- src/common/hardware_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/hardware_utils.cpp b/src/common/hardware_utils.cpp index 43e2af6b..e40742fb 100644 --- a/src/common/hardware_utils.cpp +++ b/src/common/hardware_utils.cpp @@ -37,7 +37,7 @@ bldc_motor_slots_t esp32_bldc_motor_slots[4] = { // define stepper motor slots array stepper_motor_slots_t esp32_stepper_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 }; From 3a9caf7096faab3a1dc71cc7b12486c0cd599f7b Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 16 Nov 2020 20:49:45 +0000 Subject: [PATCH 028/749] adding trap 120 and trap 150 support --- src/BLDCMotor.cpp | 59 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 55 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index a6096857..7bab5d3d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -224,6 +224,9 @@ void BLDCMotor::move(float new_target) { // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { + + const bool centered = true; + switch (foc_modulation) { case FOCModulationType::SinePWM : @@ -241,6 +244,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { Ua = Ualpha + driver->voltage_power_supply/2; Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -=Umin; + Ub -=Umin; + Uc -=Umin; + } + break; case FOCModulationType::SpaceVectorPWM : @@ -262,10 +273,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply; float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply; // two versions possible - // centered around driver->voltage_power_supply/2 - float T0 = 1 - T1 - T2; - // pulled to 0 - better for low power supply voltage - //float T0 = 0; + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_power_supply/2 + } // calculate the duty cycles(times) float Ta,Tb,Tc; @@ -312,6 +323,46 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { Ub = Tb*driver->voltage_power_supply; Uc = Tc*driver->voltage_power_supply; break; + + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + static int trap_120_state = 0; + trap_120_state = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[trap_120_state][0] * Uq; + Ub = Uq + trap_120_map[trap_120_state][1] * Uq; + Uc = Uq + trap_120_map[trap_120_state][2] * Uq; + + if (centered) { + Ua += (voltage_power_supply)/2 -Uq; + Ub += (voltage_power_supply)/2 -Uq; + Uc += (voltage_power_supply)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + static int trap_150_state = 0; + trap_150_state = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[trap_150_state][0] * Uq; + Ub = Uq + trap_150_map[trap_150_state][1] * Uq; + Uc = Uq + trap_150_map[trap_150_state][2] * Uq; + + //center + if (centered) { + Ua += (voltage_power_supply)/2 -Uq; + Ub += (voltage_power_supply)/2 -Uq; + Uc += (voltage_power_supply)/2 -Uq; + } + + break; } // set the voltages in driver From 372a9b9c6f6b92d6a8d722bcc98625f98c0c2a05 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 16 Nov 2020 20:54:13 +0000 Subject: [PATCH 029/749] Adding updated hallsensors --- src/sensors/HallSensor.cpp | 153 ++++++++++++++++--------------------- src/sensors/HallSensor.h | 28 +++++-- 2 files changed, 84 insertions(+), 97 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 7072a97a..47dc53d0 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -6,27 +6,15 @@ - hallA, hallB, hallC - HallSensor A, B and C pins - pp - pole pairs */ - HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ - // HallSensor measurement structure init // hardware pins pinA = _hallA; pinB = _hallB; pinC = _hallC; - // counter setup - pulse_counter = 0; - pulse_timestamp = 0; - - cpr = _pp * 6; // hall has 6 segments per electrical revolution - A_active = 0; - B_active = 0; - C_active = 0; - - // velocity calculation variables - prev_pulse_counter = 0; - prev_timestamp_us = _micros(); + // hall has 6 segments per electrical revolution + cpr = _pp * 6; // extern pullup as default pullup = Pullup::EXTERN; @@ -50,82 +38,61 @@ void HallSensor::handleC() { updateState(); } +/** + * Updates the state and sector following an interrupt + */ void HallSensor::updateState() { - int newState = C_active + (B_active << 1) + (A_active << 2); - Direction direction = decodeDirection(state, newState); - state = newState; - - pulse_counter += direction; - pulse_timestamp = _micros(); -} - -// determines whether the hallsensr state transition means that it has moved one step CW (+1), CCW (-1) or state transition is invalid (0) -// states are 3bits, one for each hall sensor -Direction HallSensor::decodeDirection(int oldState, int newState) -{ - // here are expected state transitions (oldState > newState). - // CW state transitions are: ( 6 > 2 > 3 > 1 > 5 > 4 > 6 ) - // CCW state transitions are: ( 6 > 4 > 5 > 1 > 3 > 2 > 6 ) - // invalid state transitions are oldState == newState or if newState or oldState == 0 | 7 as hallsensors can't be all on or all off - - int rawDirection; - - if ( - (oldState == 6 && newState == 2) || \ - (oldState == 2 && newState == 3) || \ - (oldState == 3 && newState == 1) || \ - (oldState == 1 && newState == 5) || \ - (oldState == 5 && newState == 4) || \ - (oldState == 4 && newState == 6) - ) { - rawDirection = Direction::CW; - } else if( - (oldState == 6 && newState == 4) || \ - (oldState == 4 && newState == 5) || \ - (oldState == 5 && newState == 1) || \ - (oldState == 1 && newState == 3) || \ - (oldState == 3 && newState == 2) || \ - (oldState == 2 && newState == 6) - ) { - rawDirection = Direction::CCW; + long new_pulse_timestamp = _micros(); + hall_state = C_active + (B_active << 1) + (A_active << 2); + int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; + if (new_electric_sector - electric_sector > 3) { + //underflow + direction = static_cast(natural_direction * -1); + electric_rotations += direction; + } else if (new_electric_sector - electric_sector < (-3)) { + //overflow + direction = static_cast(natural_direction); + electric_rotations += direction; } else { - rawDirection = Direction::UNKNOWN; + direction = (new_electric_sector > electric_sector)? static_cast(natural_direction) : static_cast(natural_direction * (-1)); } + electric_sector = new_electric_sector; + pulse_diff = new_pulse_timestamp - pulse_timestamp; + pulse_timestamp = new_pulse_timestamp; + total_interrupts++; + if (onSectorChange != nullptr) onSectorChange(electric_sector); +} - direction = static_cast(rawDirection * natural_direction); - return direction; // * goofy; +/** + * Optionally set a function callback to be fired when sector changes + * void onSectorChange(int sector) { + * ... // for debug or call driver directly? + * } + * sensor.attachSectorCallback(onSectorChange); + */ +void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { + onSectorChange = _onSectorChange; } /* Shaft angle calculation */ -float HallSensor::getAngle(){ - - long dN = pulse_counter - prev_pulse_counter; - - if (dN != 0) - { - - // time from last impulse - float Th = (pulse_timestamp - prev_timestamp_us) * 1e-6; - if (Th <= 0 || Th > 0.5) - Th = 1e-3; - // save variables for next pass - prev_timestamp_us = pulse_timestamp; - prev_pulse_counter = pulse_counter; - velocity = (float) _2PI * dN / (cpr * Th); - } - angle = (float) _2PI * pulse_counter / cpr; - - return angle; +float HallSensor::getAngle() { + return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI; } + /* Shaft velocity calculation function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - // this is calculated during the last call to getAngle(); - return velocity; + + if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > STALE_HALL_DATA_MICROS) ) { // last velocity isn't accurate if too old + return 0; + } else { + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + } + } // getter for index pin @@ -133,21 +100,27 @@ float HallSensor::getVelocity(){ int HallSensor::needsAbsoluteZeroSearch(){ return 0; } -// getter for index pin + int HallSensor::hasAbsoluteZero(){ - return 0; + return 1; } -// initialize counter to zero + +// set current angle as zero angle +// return the angle [rad] difference float HallSensor::initRelativeZero(){ - - pulse_counter = 0; - pulse_timestamp = _micros(); - velocity = 0; - return 0.0; + + // nothing to do. The interrupts should have changed sector. + electric_rotations = 0; + return 0; + } -// initialize index to zero + +// set absolute zero angle as zero angle +// return the angle [rad] difference float HallSensor::initAbsoluteZero(){ - return 0.0; + + return -getAngle(); + } // HallSensor initialisation of the hardware pins @@ -164,12 +137,14 @@ void HallSensor::init(){ pinMode(pinB, INPUT); pinMode(pinC, INPUT); } + + // init hall_state + A_active= digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); - // counter setup - pulse_counter = 0; pulse_timestamp = _micros(); - prev_pulse_counter = 0; - prev_timestamp_us = _micros(); } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 7eb6d1fd..0eb42286 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -6,6 +6,12 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#ifndef STALE_HALL_DATA_MICROS + #define STALE_HALL_DATA_MICROS 500000 +#endif + +// seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111 +const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 }; class HallSensor: public Sensor{ public: @@ -81,26 +87,32 @@ class HallSensor: public Sensor{ // whether last step was CW (+1) or CCW (-1) direction Direction direction; - // the current 3bit state of the hall sensors - int state; + void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); - volatile float angle; // rad/s - volatile float velocity; // rad/s + // the current 3bit state of the hall sensors + volatile int8_t hall_state; + // the current sector of the sensor. Each sector is 60deg electrical + volatile int8_t electric_sector; + // the number of electric rotations + volatile long electric_rotations; + // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) + volatile long total_interrupts; private: Direction decodeDirection(int oldState, int newState); void updateState(); - volatile long pulse_counter;//!< current pulse counter + int zero_offset; volatile long pulse_timestamp;//!< last impulse timestamp in us volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel volatile int C_active; //!< current active states of C channel - // velocity calculation variables - // float prev_Th, pulse_per_second; - volatile long prev_pulse_counter, prev_timestamp_us; + // function pointer for on sector change call back + void (*onSectorChange)(int sector) = nullptr; + + volatile long pulse_diff; }; From b3c7f24f539b18f850463b838ffa3cfa5cb36887 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 16 Nov 2020 20:57:15 +0000 Subject: [PATCH 030/749] forgot to update the enum --- src/common/base_classes/FOCMotor.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index e43dd039..3fc6d238 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -28,7 +28,9 @@ enum ControlType{ */ enum FOCModulationType{ SinePWM, //!< Sinusoidal PWM modulation - SpaceVectorPWM //!< Space vector modulation method + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 }; /** From e38a19434aedc33a8329be616a53bba822eea520 Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 16 Nov 2020 21:12:07 +0000 Subject: [PATCH 031/749] moving the case statements to avoid cross initialization --- src/BLDCMotor.cpp | 82 ++++++++++++++++++++++++----------------------- 1 file changed, 42 insertions(+), 40 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 7bab5d3d..68411f27 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -226,9 +226,50 @@ void BLDCMotor::move(float new_target) { void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { const bool centered = true; + int sector; switch (foc_modulation) { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (voltage_power_supply)/2 -Uq; + Ub += (voltage_power_supply)/2 -Uq; + Uc += (voltage_power_supply)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (voltage_power_supply)/2 -Uq; + Ub += (voltage_power_supply)/2 -Uq; + Uc += (voltage_power_supply)/2 -Uq; + } + + break; + case FOCModulationType::SinePWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation @@ -268,7 +309,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); // find the sector we are in currently - int sector = floor(angle_el / _PI_3) + 1; + sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply; float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply; @@ -324,45 +365,6 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { Uc = Tc*driver->voltage_power_supply; break; - case FOCModulationType::Trapezoid_120 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 - static int trap_120_map[6][3] = { - {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - static int trap_120_state = 0; - trap_120_state = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - - Ua = Uq + trap_120_map[trap_120_state][0] * Uq; - Ub = Uq + trap_120_map[trap_120_state][1] * Uq; - Uc = Uq + trap_120_map[trap_120_state][2] * Uq; - - if (centered) { - Ua += (voltage_power_supply)/2 -Uq; - Ub += (voltage_power_supply)/2 -Uq; - Uc += (voltage_power_supply)/2 -Uq; - } - break; - - case FOCModulationType::Trapezoid_150 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 - static int trap_150_map[12][3] = { - {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - static int trap_150_state = 0; - trap_150_state = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - - Ua = Uq + trap_150_map[trap_150_state][0] * Uq; - Ub = Uq + trap_150_map[trap_150_state][1] * Uq; - Uc = Uq + trap_150_map[trap_150_state][2] * Uq; - - //center - if (centered) { - Ua += (voltage_power_supply)/2 -Uq; - Ub += (voltage_power_supply)/2 -Uq; - Uc += (voltage_power_supply)/2 -Uq; - } - - break; } // set the voltages in driver From 8b44473da1e353a850ff7bf02f0747f1ae98c1f1 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 18 Nov 2020 20:11:06 +0100 Subject: [PATCH 032/749] FEAT esp32 and stm32 stable 6PWM configuration --- src/drivers/BLDCDriver6PWM.cpp | 6 +- src/drivers/StepperDriver4PWM.cpp | 2 +- .../hardware_specific/atmega328_mcu.cpp | 8 +- src/drivers/hardware_specific/esp32_mcu.cpp | 78 +++++++------------ src/drivers/hardware_specific/stm32_mcu.cpp | 70 +++++++++++------ 5 files changed, 82 insertions(+), 82 deletions(-) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index a973fd33..b1f2e884 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -69,9 +69,9 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { Uc = _constrain(Uc, -voltage_limit, voltage_limit); // calculate duty cycle // limited in [0,1] - float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 ); - float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 ); - float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index e661ed43..e7eec6ff 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -53,7 +53,7 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure4PWM(pwm_frequency, pwm1A, pwm2A, pwm1B, pwm2B); + _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); return 0; } diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index f5374ebe..58b368f7 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -66,7 +66,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm -int _configurePair(int pinH, int pinL) { +int _configureComplementaryPair(int pinH, int pinL) { if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); @@ -101,9 +101,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // High PWM frequency // - always max 32kHz int ret_flag = 0; - ret_flag += _configurePair(pinA_h, pinA_l); - ret_flag += _configurePair(pinB_h, pinB_l); - ret_flag += _configurePair(pinC_h, pinC_l); + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); return ret_flag; // returns -1 if not well configured } diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 5030ac11..b8ce1d39 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -68,41 +68,27 @@ bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { }; // configuring high frequency pwm timer +// a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ mcpwm_config_t pwm_config; pwm_config.frequency = pwm_frequency; //frequency - pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 50.0% - pwm_config.cmpr_b = 0; //duty cycle of PWMxB = 50.0% pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH 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 - _delay(100); - - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); - mcpwm_num->clk_cfg.prescale = 0; + if (dead_zone != NOT_SET){ + // dead zone is configured in dead_time x 100 nanoseconds + float dead_time = (float)(1e7 / pwm_frequency) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + } - mcpwm_num->timer[0].period.prescale = 4; - mcpwm_num->timer[1].period.prescale = 4; - mcpwm_num->timer[2].period.prescale = 4; - _delay(1); - mcpwm_num->timer[0].period.period = 2048; - mcpwm_num->timer[1].period.period = 2048; - mcpwm_num->timer[2].period.period = 2048; - _delay(1); - mcpwm_num->timer[0].period.upmethod = 0; - mcpwm_num->timer[1].period.upmethod = 0; - mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + _delay(100); 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); @@ -121,7 +107,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency bldc_3pwm_motor_slots_t m_slot = {}; @@ -164,7 +150,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency stepper_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters @@ -210,10 +196,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB for(int i = 0; i < 4; i++){ if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,2047] - esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047; - esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047; - esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047; + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); break; } } @@ -228,11 +214,11 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in for(int i = 0; i < 2; i++){ if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,2047] - esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1a*2047; - esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1b*2047; - esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2a*2047; - esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2b*2047; + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2b*100.0); break; } } @@ -257,7 +243,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } } // if no slots available - if(!m_slot.mcpwm_unit) return -1; + if(slot_num >= 2) return -1; // disable all the slots with the same MCPWM if( slot_num == 0 ){ @@ -283,7 +269,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); // return return 0; } @@ -297,18 +283,12 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0; - float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0; - float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0; - float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0; - float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0; - float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0; - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_ah); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_al); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_bh); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_bl); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_ch); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_cl); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100); break; } } diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 4fa8e0c9..85d0bd65 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -65,7 +65,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - TIM_OC_InitTypeDef sConfigOC = {0}; + TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); sConfigOC.OCMode = TIM_OCMODE_PWM2; sConfigOC.Pulse = 100; sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; @@ -117,7 +117,7 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha HT4->resume(); } -HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, int dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { PinName uhPinName = digitalPinToPinName(pinA_h); PinName ulPinName = digitalPinToPinName(pinA_l); @@ -144,8 +144,11 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, int dead_zone, int HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); - - LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, 100); // deadtime is non linear! + + // dead time is set in nanoseconds + uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); HT->pause(); @@ -237,35 +240,52 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, 100000); // constrain to 100kHz max + // find configuration int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - - if(config == _HARDWARE_6PWM){ - _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - }else if(config == _SOFTWARE_6PWM){ - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); - _initPinPWMLow(pwm_frequency, pinA_l); - HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); - _initPinPWMLow(pwm_frequency, pinB_l); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); - _initPinPWMLow(pwm_frequency, pinC_l); - _alignPWMTimers(HT1, HT2, HT3); - }else{ - return -1; + // configure accordingly + switch(config){ + case _ERROR_6PWM: + return -1; // fail + break; + case _HARDWARE_6PWM: + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + break; + case _SOFTWARE_6PWM: + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + break; } - return 0; + return 0; // success } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwm(pinA_l, _constrain(dc_a + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM - _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM - _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM - _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // set pwm accordingly + switch(config){ + case _HARDWARE_6PWM: + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + break; + } } #endif \ No newline at end of file From 0e537c8cdec32e2aa2d44c0753fabba4ba47969d Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 18 Nov 2020 20:12:21 +0100 Subject: [PATCH 033/749] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index f3f4d9e4..19ed47bf 100644 --- a/README.md +++ b/README.md @@ -181,16 +181,16 @@ You can find more details in the [SimpleFOC documentation](https://docs.simplefo Here are some of the *Simple**FOC**library* and *Simple**FOC**Shield* application examples.

      - + - + - + - +

      From 640d8500d19d0859bd50fc55aee98c854d42164c Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 15:41:21 +0100 Subject: [PATCH 034/749] FEAT d components added --- keywords.txt | 1 + src/BLDCMotor.cpp | 63 +++++++++++---------- src/BLDCMotor.h | 3 +- src/StepperMotor.cpp | 44 ++++++-------- src/StepperMotor.h | 3 +- src/common/base_classes/FOCMotor.cpp | 7 +-- src/common/base_classes/FOCMotor.h | 2 +- src/drivers/hardware_specific/esp32_mcu.cpp | 12 ++-- 8 files changed, 65 insertions(+), 70 deletions(-) diff --git a/keywords.txt b/keywords.txt index 77966b94..6540170d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -62,6 +62,7 @@ angleOpenloop KEYWORD2 velocityOpenloop KEYWORD2 voltage_q KEYWORD2 +voltage_d KEYWORD2 shaft_angle_sp KEYWORD2 shaft_velocity_sp KEYWORD2 shaft_angle KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 68411f27..bed4cd6a 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -33,7 +33,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { void BLDCMotor::init() { if(monitor_port) monitor_port->println("MOT: Initialise variables."); // sanity check for the voltage limit configuration - if(voltage_limit > driver->voltage_power_supply) voltage_limit = driver->voltage_power_supply; + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; // update the controller limits @@ -98,13 +98,13 @@ int BLDCMotor::alignSensor() { float start_angle = shaftAngle(); for (int i = 0; i <=5; i++ ) { float angle = _3PI_2 + _2PI * i / 6.0; - setPhaseVoltage(voltage_sensor_align, angle); + setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(200); } float mid_angle = shaftAngle(); for (int i = 5; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 6.0; - setPhaseVoltage(voltage_sensor_align, angle); + setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(200); } if (mid_angle < start_angle) { @@ -119,7 +119,7 @@ int BLDCMotor::alignSensor() { // set sensor to zero sensor->initRelativeZero(); _delay(500); - setPhaseVoltage(0,0); + setPhaseVoltage(0, 0, 0); _delay(200); // find the index if available @@ -151,7 +151,7 @@ int BLDCMotor::absoluteZeroAlign() { } voltage_q = 0; // disable motor - setPhaseVoltage(0,0); + setPhaseVoltage(0, 0, 0); // align absolute zero if it has been found if(!sensor->needsAbsoluteZeroSearch()){ @@ -170,7 +170,7 @@ void BLDCMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); } // Iterative function running outer loop of the FOC algorithm @@ -217,16 +217,17 @@ void BLDCMotor::move(float new_target) { } -// Method using FOC to set Uq to the motor at the optimal angle +// Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Space Vector PWM and Sine PWM algorithms // // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) -void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { const bool centered = true; int sector; + float _ca,_sa; switch (foc_modulation) { @@ -243,9 +244,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { Uc = Uq + trap_120_map[sector][2] * Uq; if (centered) { - Ua += (voltage_power_supply)/2 -Uq; - Ub += (voltage_power_supply)/2 -Uq; - Uc += (voltage_power_supply)/2 -Uq; + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; } break; @@ -263,9 +264,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { //center if (centered) { - Ua += (voltage_power_supply)/2 -Uq; - Ub += (voltage_power_supply)/2 -Uq; - Uc += (voltage_power_supply)/2 -Uq; + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; } break; @@ -277,20 +278,22 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); // Inverse park transform - Ualpha = -_sin(angle_el) * Uq; // -sin(angle) * Uq; - Ubeta = _cos(angle_el) * Uq; // cos(angle) * Uq; + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; // Clarke transform - Ua = Ualpha + driver->voltage_power_supply/2; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_power_supply/2; + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; if (!centered) { float Umin = min(Ua, min(Ub, Uc)); - Ua -=Umin; - Ub -=Umin; - Uc -=Umin; + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; } break; @@ -311,12 +314,12 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { // find the sector we are in currently sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply; + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (centered) { - T0 = 1 - T1 - T2; //centered around driver->voltage_power_supply/2 + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 } // calculate the duty cycles(times) @@ -360,9 +363,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) { } // calculate the phase voltages and center - Ua = Ta*driver->voltage_power_supply; - Ub = Tb*driver->voltage_power_supply; - Uc = Tc*driver->voltage_power_supply; + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; break; } @@ -386,7 +389,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ shaft_angle += target_velocity*Ts; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; @@ -409,7 +412,7 @@ void BLDCMotor::angleOpenloop(float target_angle){ shaft_angle = target_angle; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 4ad76d89..164cc108 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -76,9 +76,10 @@ class BLDCMotor: public FOCMotor * Heart of the FOC algorithm * * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float angle_el); + void setPhaseVoltage(float Uq, float Ud, float angle_el); /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 590a895d..e8e991cf 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -24,7 +24,7 @@ void StepperMotor::init() { if(monitor_port) monitor_port->println("MOT: Init variables."); // sanity check for the voltage limit configuration - if(voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; @@ -91,13 +91,13 @@ int StepperMotor::alignSensor() { float start_angle = shaftAngle(); for (int i = 0; i <=5; i++ ) { float angle = _3PI_2 + _2PI * i / 6.0; - setPhaseVoltage(voltage_sensor_align, angle); + setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(200); } float mid_angle = shaftAngle(); for (int i = 5; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 6.0; - setPhaseVoltage(voltage_sensor_align, angle); + setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(200); } if (mid_angle < start_angle) { @@ -112,7 +112,7 @@ int StepperMotor::alignSensor() { // set sensor to zero sensor->initRelativeZero(); _delay(500); - setPhaseVoltage(0,0); + setPhaseVoltage(0, 0, 0); _delay(200); // find the index if available @@ -143,8 +143,9 @@ int StepperMotor::absoluteZeroAlign() { voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); } voltage_q = 0; + voltage_d = 0; // disable motor - setPhaseVoltage(0,0); + setPhaseVoltage(0, 0, 0); // align absolute zero if it has been found if(!sensor->needsAbsoluteZeroSearch()){ @@ -163,7 +164,7 @@ void StepperMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle, pole_pairs)); } // Iterative function running outer loop of the FOC algorithm @@ -210,36 +211,25 @@ void StepperMotor::move(float new_target) { } -// Method using FOC to set Uq to the motor at the optimal angle -// Function implementingSine PWM algorithms +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Sine PWM algorithms // - space vector not implemented yet // // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) -void StepperMotor::setPhaseVoltage(float Uq, float angle_el) { +void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + zero_electric_angle); + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); // Inverse park transform - Ualpha = -(_sin(angle_el)) * Uq; // -sin(angle) * Uq; - Ubeta = (_cos(angle_el)) * Uq; // cos(angle) * Uq; - // if (angle_el < _PI_2) { - // Ualpha = 0; - // Ubeta = Uq; - // }else if (angle_el < _PI) { - // Ualpha =- Uq; - // Ubeta = 0; - // }else if (angle_el < _3PI_2) { - // Ualpha = 0; - // Ubeta = -Uq; - // }else{ - // Ualpha = Uq; - // Ubeta = 0; - // } + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; // set the voltages in hardware driver->setPwm(Ualpha, Ubeta); @@ -258,7 +248,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){ shaft_angle += target_velocity*Ts; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; @@ -281,7 +271,7 @@ void StepperMotor::angleOpenloop(float target_angle){ shaft_angle = target_angle; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7786e4d7..af4783bf 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -84,9 +84,10 @@ class StepperMotor: public FOCMotor * Heart of the FOC algorithm * * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float angle_el); + void setPhaseVoltage(float Uq, float Ud , float angle_el); /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 591463aa..1d1a0f43 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -5,13 +5,10 @@ */ FOCMotor::FOCMotor() { - // Power supply voltage - voltage_power_supply = DEF_POWER_SUPPLY; - // maximum angular velocity to be used for positioning velocity_limit = DEF_VEL_LIM; // maximum voltage to be set to the motor - voltage_limit = voltage_power_supply; + voltage_limit = DEF_POWER_SUPPLY; // index search velocity velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; @@ -26,6 +23,8 @@ FOCMotor::FOCMotor() // default target value target = 0; + voltage_d = 0; + voltage_q = 0; //monitor_port monitor_port = nullptr; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 3fc6d238..986e1ab2 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -103,9 +103,9 @@ class FOCMotor float shaft_velocity_sp;//!< current target velocity float shaft_angle_sp;//!< current target angle float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set // motor configuration parameters - float voltage_power_supply;//!< Power supply voltage float voltage_sensor_align;//!< sensor and motor align voltage parameter float velocity_index_search;//!< target velocity for index search int pole_pairs;//!< Motor pole pairs number diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index b8ce1d39..3c07a445 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -283,12 +283,12 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); break; } } From 710819a43fd6b58724242ea88a480cc2cfe2e813 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 15:52:11 +0100 Subject: [PATCH 035/749] FIX pid error fixed from previous to current --- src/common/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 5275c728..1a0a9828 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -25,7 +25,7 @@ float PIDController::operator() (float error){ // Discrete implementations // proportional part // u_p = P *e(k) - float proportional = P * error_prev; + float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) float integral = integral_prev + I*Ts*0.5*(error + error_prev); From e92d911dbc4c878b3ed50fd2108ba5d4c814c690 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 16:05:22 +0100 Subject: [PATCH 036/749] FEAT restructured i2c and spi examples --- .github/workflows/ccpp.yml | 2 +- .../esp32_i2c_dual_bus_example.ino} | 19 ++--------- .../stm32_i2c_dual_bus_example.ino | 32 +++++++++++++++++++ 3 files changed, 36 insertions(+), 17 deletions(-) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino => magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino} (54%) create mode 100644 examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index ae273238..98e81cb3 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino similarity index 54% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index 27cce1ca..c781847f 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -7,17 +7,6 @@ MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); -#if defined(_STM32_DEF_) // if stm chips - // example of stm32 defining 2nd bus - TwoWire Wire1(PB11, PB10); - -#elif defined(ESP_H) // if esp32 - // esp32 defines a Wire1 but doesn't define pins! - // nothing to do here for esp32! (See below) -#else - // Wire constructors vary - you'll have to check what works for your chip - TwoWire Wire1(SDA1, SCL1); -#endif void setup() { @@ -27,11 +16,9 @@ void setup() { Wire.setClock(400000); Wire1.setClock(400000); - #if defined(ESP_H) // if esp32 - // Normally SimpeFOC will call begin for i2c but with esp32 begin() is the only way to set pins! - // It seems safe to call begin multiple times - Wire1.begin(19,23,400000); - #endif + // Normally SimpleFOC will call begin for i2c but with esp32 begin() is the only way to set pins! + // It seems safe to call begin multiple times + Wire1.begin(19, 23, 400000); sensor0.init(); sensor1.init(&Wire1); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino new file mode 100644 index 00000000..019e088c --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino @@ -0,0 +1,32 @@ +#include + +/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus + * This example shows how a second i2c bus can be used to communicate with a second sensor. + */ + +MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); +MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); + +// example of stm32 defining 2nd bus +TwoWire Wire1(PB11, PB10); + + +void setup() { + + Serial.begin(115200); + _delay(750); + + Wire.setClock(400000); + Wire1.setClock(400000); + + sensor0.init(); + sensor1.init(&Wire1); +} + +void loop() { + _delay(200); + Serial.print(sensor0.getAngle()); + Serial.print(" - "); + Serial.print(sensor1.getAngle()); + Serial.println(); +} From 32f509f8592a6bac4ece74e8c3c7a31f9dd95b86 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 16:20:05 +0100 Subject: [PATCH 037/749] version v2.0 --- README.md | 13 ++++++++++--- .../esp32_position_control.ino | 6 ++++-- library.properties | 2 +- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 19ed47bf..57bf9a83 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -

      NEW RELEASE 📢: SimpleFOClibrary v1.7.0

        +

        NEW RELEASE 📢: SimpleFOClibrary v2.0

        • 6PWM support
          • Arduino UNO (atmega328)
            • @@ -27,9 +27,16 @@ Therefore this is an attempt to:
          • PWM config
          • -
        • Refactoring
          • +
        • I2C and SPI sensors multiple busses support by @owennewo +
          • +
        • Added support for separate setting of Ud and Uq setting. +
            +
          • Preparations for current control
            • +
          • Working only for SinePWM modulation at the moment
            • +
          • +
        • A lot of refactoring
        -The library version 1.7.0 will be released once when it is properly tested and documented! +The library version v2.0 will be released once when it is properly tested and documented!
        ## Arduino *SimpleFOCShield* diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index ed5dbef9..617f04fc 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -7,13 +7,15 @@ // MISO 12 // MOSI 9 // SCK 14 -MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); // I2C Magnetic sensor instance (AS5600 example) // make sure to use the pull-ups!! // SDA 21 // SCL 22 -//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); +// magnetic sensor instance - I2C +//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); // Analog output Magnetic sensor instance (AS5600) // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); diff --git a/library.properties b/library.properties index 4b72b9d3..b88023a9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=1.7.0 +version=2.0.0 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 3c689bd009664fd6ff27fb5749599c7a909f652f Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 16:26:28 +0100 Subject: [PATCH 038/749] TYPO register --- src/sensors/MagneticSensorSPI.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 006a6171..58660e0b 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -31,7 +31,7 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r chip_select_pin = cs; // angle read register of the magnetic sensor - angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTAR; + angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) cpr = pow(2,_bit_resolution); spi_mode = SPI_MODE1; @@ -47,7 +47,7 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ chip_select_pin = cs; // angle read register of the magnetic sensor - angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTAR; + angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) cpr = pow(2, config.bit_resolution); spi_mode = config.spi_mode; From 624935fd16150c0cdccf9ba9f8c2d3a106b8ad8b Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 21 Nov 2020 16:30:53 +0100 Subject: [PATCH 039/749] README update + forgotten save register typo --- README.md | 8 +++++++- src/sensors/MagneticSensorSPI.h | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 57bf9a83..71db3108 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Therefore this is an attempt to:
      • esp32 boards
      • -
    • BLDC driver code separated +
    • BLDC driver code separated
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
        • @@ -29,6 +29,12 @@ Therefore this is an attempt to:
      • I2C and SPI sensors multiple busses support by @owennewo
        • +
      • Initial implementation of Block commutation by @owennewo +
          +
        • FOCModulationType::Trapezoid_120
          • +
        • FOCModulationType::Trapezoid_150
          • +
        +
      • Added support for separate setting of Ud and Uq setting.
        • Preparations for current control
          • diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 16b0a9f2..05fb9091 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -7,7 +7,7 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" -#define DEF_ANGLE_REGISTAR 0x3FFF +#define DEF_ANGLE_REGISTER 0x3FFF struct MagneticSensorSPIConfig_s { int spi_mode; From 51f0f7935d3761fd31b9c26d69c80ff0095f4184 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 12:01:13 +0100 Subject: [PATCH 040/749] FEAT added standalone driver examples --- .../open_loop_velocity_example.ino | 2 +- .../bldc_driver_3pwm_standalone.ino | 33 +++++++++++++++++ .../bldc_driver_6pwm_standalone.ino | 35 +++++++++++++++++++ .../stepper_driver_4pwm_standalone.ino | 33 +++++++++++++++++ 4 files changed, 102 insertions(+), 1 deletion(-) create mode 100644 examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino create mode 100644 examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino create mode 100644 examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 9e8085dd..7c272097 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -1,5 +1,5 @@ // Open loop motor control example - #include +#include // BLDC motor & driver instance diff --git a/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino new file mode 100644 index 00000000..65ef5807 --- /dev/null +++ b/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -0,0 +1,33 @@ +// BLDC driver standalone example +#include + + +// BLDC driver instance +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +void setup() { + + // pwm frequency to be used [Hz] + // for atmega328 fixed to 32kHz + // esp32/stm32/teensy configurable + driver.pwm_frequency = 50000; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + // phase C: 5V + driver.setPwm(3,6,5); +} \ No newline at end of file diff --git a/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino new file mode 100644 index 00000000..4a986654 --- /dev/null +++ b/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -0,0 +1,35 @@ +// BLDC driver standalone example +#include + +// BLDC driver instance +BLDCDriver3PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); + +void setup() { + + // pwm frequency to be used [Hz] + // for atmega328 fixed to 32kHz + // esp32/stm32/teensy configurable + driver.pwm_frequency = 50000; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + // daad_zone [0,1] - default 0.02 - 2% + driver.dead_zone = 0.05; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + // phase C: 5V + driver.setPwm(3,6,5); +} \ No newline at end of file diff --git a/examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino new file mode 100644 index 00000000..7ac5ac62 --- /dev/null +++ b/examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -0,0 +1,33 @@ +// Stepper driver standalone example +#include + + +// Stepper driver instance +StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); + +void setup() { + + // pwm frequency to be used [Hz] + // for atmega328 fixed to 32kHz + // esp32/stm32/teensy configurable + driver.pwm_frequency = 50000; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + driver.setPwm(3,6); +} \ No newline at end of file From 1b98f79a803b744506200d83d0c78385921d03a6 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 12:05:30 +0100 Subject: [PATCH 041/749] FIX typo in the example --- .../bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino | 1 + .../bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 65ef5807..1235fccd 100644 --- a/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -15,6 +15,7 @@ void setup() { driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply driver.voltage_limit = 12; + // driver init driver.init(); diff --git a/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 4a986654..972a74db 100644 --- a/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -2,7 +2,7 @@ #include // BLDC driver instance -BLDCDriver3PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); +BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { From c678744ac4f710ee60b4cf5a0832668facf80b15 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 12:06:35 +0100 Subject: [PATCH 042/749] rename of stanalone examples --- .../bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino | 0 .../bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino | 0 .../stepper_driver_4pwm_standalone.ino | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename examples/utils/{driver_test => driver_standalone_test}/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino (100%) rename examples/utils/{driver_test => driver_standalone_test}/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino (100%) rename examples/utils/{driver_test => driver_standalone_test}/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino (100%) diff --git a/examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino similarity index 100% rename from examples/utils/driver_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino rename to examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino diff --git a/examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino similarity index 100% rename from examples/utils/driver_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino rename to examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino diff --git a/examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino similarity index 100% rename from examples/utils/driver_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino rename to examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino From 5b85e99ef7139d65f761f0d0ad15ad7960af82af Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 18:07:39 +0100 Subject: [PATCH 043/749] Update readme --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 71db3108..5b8adf76 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,6 @@ Therefore this is an attempt to:
      • A lot of refactoring
      -The library version v2.0 will be released once when it is properly tested and documented!
    ## Arduino *SimpleFOCShield* From 10a8a4099a4846f6b6f27c6dcd6872b33d6959db Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 18:11:35 +0100 Subject: [PATCH 044/749] README update --- README.md | 33 +-------------------------------- 1 file changed, 1 insertion(+), 32 deletions(-) diff --git a/README.md b/README.md index 5b8adf76..44ce42e2 100644 --- a/README.md +++ b/README.md @@ -11,38 +11,7 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -

    NEW RELEASE 📢: SimpleFOClibrary v2.0

      -
    • 6PWM support -
        -
      • Arduino UNO (atmega328)
        • -
      • stm32 boards
        • -
      • esp32 boards
        • -
      -
      • -
    • BLDC driver code separated -
        -
      • BLDC: 6pwm and 3pwm
        • -
      • Stepper: 4pwm
        • -
      • Hardware specific code in separate files
        • -
      • PWM config
        • -
      -
      • -
    • I2C and SPI sensors multiple busses support by @owennewo -
      • -
    • Initial implementation of Block commutation by @owennewo -
        -
      • FOCModulationType::Trapezoid_120
        • -
      • FOCModulationType::Trapezoid_150
        • -
      -
      • -
    • Added support for separate setting of Ud and Uq setting. -
        -
      • Preparations for current control
        • -
      • Working only for SinePWM modulation at the moment
        • -
      • -
    • A lot of refactoring
      • -
    -
    +

    NEW RELEASE 📢: SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    The library version v2.0 will be released once when it is properly tested and documented!
    ## Arduino *SimpleFOCShield* From 3e577d34228722b8b1441ac99444491108a7e6d1 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 22 Nov 2020 18:11:59 +0100 Subject: [PATCH 045/749] typo readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 44ce42e2..eab04a05 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -

    NEW RELEASE 📢: SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    The library version v2.0 will be released once when it is properly tested and documented!
    +

    NEW RELEASE 📢: SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    ## Arduino *SimpleFOCShield* From 639f57758f3eaf174c4da43a7893e850b3e35dbd Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Mon, 23 Nov 2020 19:18:31 +0000 Subject: [PATCH 046/749] more stable velocity for hall sensor --- src/sensors/HallSensor.cpp | 13 ++++++++++--- src/sensors/HallSensor.h | 4 ---- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 47dc53d0..2c33a41e 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -45,6 +45,7 @@ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); hall_state = C_active + (B_active << 1) + (A_active << 2); int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; + static Direction old_direction; if (new_electric_sector - electric_sector > 3) { //underflow direction = static_cast(natural_direction * -1); @@ -57,9 +58,16 @@ void HallSensor::updateState() { direction = (new_electric_sector > electric_sector)? static_cast(natural_direction) : static_cast(natural_direction * (-1)); } electric_sector = new_electric_sector; - pulse_diff = new_pulse_timestamp - pulse_timestamp; + if (direction == old_direction) { + // not oscilating or just changed direction + pulse_diff = new_pulse_timestamp - pulse_timestamp; + } else { + pulse_diff = 0; + } + pulse_timestamp = new_pulse_timestamp; total_interrupts++; + old_direction = direction; if (onSectorChange != nullptr) onSectorChange(electric_sector); } @@ -86,8 +94,7 @@ float HallSensor::getAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - - if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > STALE_HALL_DATA_MICROS) ) { // last velocity isn't accurate if too old + if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 0eb42286..ad5673e3 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -6,10 +6,6 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" -#ifndef STALE_HALL_DATA_MICROS - #define STALE_HALL_DATA_MICROS 500000 -#endif - // seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111 const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 }; From be49818d1d1bc4ec407584a2e0d3ab09778d57f9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 29 Nov 2020 12:20:38 +0100 Subject: [PATCH 047/749] ESP32 support for freq change --- src/drivers/BLDCDriver3PWM.cpp | 12 ++-- src/drivers/BLDCDriver6PWM.cpp | 6 +- src/drivers/StepperDriver4PWM.cpp | 12 ++-- src/drivers/hardware_specific/esp32_mcu.cpp | 77 +++++++++++++++++---- 4 files changed, 78 insertions(+), 29 deletions(-) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index bacb4ec5..5932ae3d 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -56,14 +56,14 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver - Ua = _constrain(Ua, -voltage_limit, voltage_limit); - Ub = _constrain(Ub, -voltage_limit, voltage_limit); - Uc = _constrain(Uc, -voltage_limit, voltage_limit); + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); // calculate duty cycle // limited in [0,1] - float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 ); - float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 ); - float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index b1f2e884..76002395 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -64,9 +64,9 @@ int BLDCDriver6PWM::init() { // Set voltage to the pwm pin void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver - Ua = _constrain(Ua, -voltage_limit, voltage_limit); - Ub = _constrain(Ub, -voltage_limit, voltage_limit); - Uc = _constrain(Uc, -voltage_limit, voltage_limit); + Ua = _constrain(Ua, 0, voltage_limit); + Ub = _constrain(Ub, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index e7eec6ff..5ddaa5d2 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -62,18 +62,18 @@ int StepperDriver4PWM::init() { void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); // limit the voltage in driver - Ualpha = _constrain(Ualpha,-voltage_limit,voltage_limit); - Ubeta = _constrain(Ubeta,-voltage_limit,voltage_limit); + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0,1); + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0,1); + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0,1); + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0,1); + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); } \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 3c07a445..f2daeadf 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -10,6 +10,17 @@ #define _EMPTY_SLOT -20 #define _TAKEN_SLOT -21 +// ABI bus frequency - would be better to take it from somewhere +// but I did nto find a good exposed variable +#define _MCPWM_FREQ 160e6 + +// preferred pwm resolution default +#define _PWM_RES_DEF 2048 +// min resolution +#define _PWM_RES_MIN 1500 +// max resolution +#define _PWM_RES_MAX 3000 + // structure containing motor slot configuration // this library supports up to 4 motors typedef struct { @@ -73,23 +84,61 @@ bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ mcpwm_config_t pwm_config; - pwm_config.frequency = pwm_frequency; //frequency pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH 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 - + if (dead_zone != NOT_SET){ - // dead zone is configured in dead_time x 100 nanoseconds - float dead_time = (float)(1e7 / pwm_frequency) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); } - _delay(100); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); + + // manual configuration due to the lack of config flexibility in mcpwm_init() + mcpwm_num->clk_cfg.prescale = 0; + // calculate prescaler and period + // step 1: calculate the prescaler using the default pwm resolution + // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 + int prescaler = ceil(_MCPWM_FREQ / _PWM_RES_DEF / 2.0 / pwm_frequency) - 1; + // constrain prescaler + prescaler = _constrain(prescaler, 0, 128); + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) + int resolution_corrected = _MCPWM_FREQ / 2.0 / pwm_frequency / (prescaler + 1); + // if pwm resolution too low lower the prescaler + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = _MCPWM_FREQ / 2.0 / pwm_frequency / (--prescaler + 1); + resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); + + // set prescaler + mcpwm_num->timer[0].period.prescale = prescaler; + mcpwm_num->timer[1].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; + _delay(1); + //set period + mcpwm_num->timer[0].period.period = resolution_corrected; + mcpwm_num->timer[1].period.period = resolution_corrected; + mcpwm_num->timer[2].period.period = resolution_corrected; + _delay(1); + mcpwm_num->timer[0].period.upmethod = 0; + mcpwm_num->timer[1].period.upmethod = 0; + mcpwm_num->timer[2].period.upmethod = 0; + _delay(1); + //restart the timers + mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + _delay(1); + 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); @@ -106,8 +155,8 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // supports Arudino/ATmega328, STM32 and ESP32 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || pwm_frequency == NOT_SET) 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 bldc_3pwm_motor_slots_t m_slot = {}; @@ -149,8 +198,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || pwm_frequency == NOT_SET) 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 stepper_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters @@ -229,8 +278,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + if(!pwm_frequency || pwm_frequency == NOT_SET) 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 bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters From ea128de3b9134f682d64358531d38ab572c69a45 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 29 Nov 2020 12:24:51 +0100 Subject: [PATCH 048/749] v2.0.1 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index b88023a9..ba0bcaca 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.0.0 +version=2.0.1 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From b5bdbdaabca0a8e020937092af3aee13a3001b21 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 30 Nov 2020 12:07:40 -0600 Subject: [PATCH 049/749] Fixed typos on examples --- .../open_loop_position_example.ino | 29 ++++++++++--------- .../open_loop_velocity_example.ino | 28 +++++++++--------- 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 900f9501..10d0c5c1 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -1,16 +1,19 @@ -// Open loop motor control example +// Open loop motor control example #include // BLDC motor & driver instance +// BLDCMotor motor = BLDCMotor(pole pair number); BLDCMotor motor = BLDCMotor(11); +// BLDCMotor motor = BLDCMotor(pole pair number); BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + // Stepper motor & driver instance //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -19,24 +22,24 @@ void setup() { motor.linkDriver(&driver); // limiting motor movements - motor.voltage_limit = 3; // rad/s - motor.velocity_limit = 20; // rad/s + motor.voltage_limit = 3; // [V] + motor.velocity_limit = 20; // [rad/s] // open loop control config motor.controller = ControlType::angle_openloop; // init motor hardware motor.init(); - + Serial.begin(115200); Serial.println("Motor ready!"); _delay(1000); } -float target_position = 0; // rad/s +float target_position = 0; // [rad/s] void loop() { - // open loop angle movements + // open loop angle movements // using motor.voltage_limit and motor.velocity_limit motor.move(target_position); @@ -47,10 +50,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -58,14 +61,14 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_position = received_chars.toFloat(); Serial.print("Target position: "); Serial.println(target_position); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } -} \ No newline at end of file +} diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 7c272097..e5f99817 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -1,16 +1,19 @@ -// Open loop motor control example +// Open loop motor control example #include // BLDC motor & driver instance +// BLDCMotor motor = BLDCMotor(pole pair number); BLDCMotor motor = BLDCMotor(11); +// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + // Stepper motor & driver instance //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); void setup() { - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -19,25 +22,24 @@ void setup() { motor.linkDriver(&driver); // limiting motor movements - motor.voltage_limit = 3; // rad/s - motor.velocity_limit = 20; // rad/s + motor.voltage_limit = 3; // [V] + motor.velocity_limit = 20; // [rad/s] // open loop control config motor.controller = ControlType::velocity_openloop; // init motor hardware motor.init(); - Serial.begin(115200); Serial.println("Motor ready!"); _delay(1000); } -float target_velocity = 0; // rad/s +float target_velocity = 0; // [rad/s] void loop() { - // open loop velocity movement + // open loop velocity movement // using motor.voltage_limit and motor.velocity_limit motor.move(target_velocity); @@ -48,10 +50,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -59,14 +61,14 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_velocity = received_chars.toFloat(); Serial.print("Target velocity "); Serial.println(target_velocity); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } -} \ No newline at end of file +} From 50a758bd32d6aa65fc6d3b6708a63019e4dc6ab2 Mon Sep 17 00:00:00 2001 From: ATILIUS-REGULUS Date: Tue, 1 Dec 2020 13:08:08 +0100 Subject: [PATCH 050/749] check for floating point error due to insufficient precision --- src/BLDCMotor.cpp | 54 ++++++++++++++++++++++++++--------------------- 1 file changed, 30 insertions(+), 24 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index bed4cd6a..13444638 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void BLDCMotor::init() { if(monitor_port) monitor_port->println("MOT: Initialise variables."); // sanity check for the voltage limit configuration @@ -53,13 +53,13 @@ void BLDCMotor::disable() { // set zero to PWM driver->setPwm(0, 0, 0); - // disable the driver + // disable the driver driver->disable(); } // enable motor driver void BLDCMotor::enable() { - // enable the driver + // enable the driver driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); @@ -93,7 +93,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: Align sensor."); // align the electrical phases of the motor and sensor - // set angle -90 degrees + // set angle -90 degrees float start_angle = shaftAngle(); for (int i = 0; i <=5; i++ ) { @@ -134,19 +134,19 @@ int BLDCMotor::alignSensor() { } -// Encoder alignment the absolute zero angle +// Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroAlign() { if(monitor_port) monitor_port->println("MOT: Absolute zero align."); // if no absolute zero return if(!sensor->hasAbsoluteZero()) return 0; - + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ - loopFOC(); + loopFOC(); voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); } voltage_q = 0; @@ -167,9 +167,9 @@ int BLDCMotor::absoluteZeroAlign() { // 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 shaft_angle = shaftAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); } @@ -219,7 +219,7 @@ void BLDCMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Space Vector PWM and Sine PWM algorithms -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) @@ -257,7 +257,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { }; // static int trap_150_state = 0; sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - + Ua = Uq + trap_150_map[sector][0] * Uq; Ub = Uq + trap_150_map[sector][1] * Uq; Uc = Uq + trap_150_map[sector][2] * Uq; @@ -272,7 +272,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SinePWM : - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park + Clarke transformation // angle normalization in between 0 and 2pi @@ -299,10 +299,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm // https://www.youtube.com/watch?v=QMSWUMEAejg - // if negative voltages change inverse the phase + // if negative voltages change inverse the phase // angle + 180degrees if(Uq < 0) angle_el += _PI; Uq = abs(Uq); @@ -316,14 +316,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; - // two versions possible + // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage - if (centered) { + if (centered) { T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 - } + } // calculate the duty cycles(times) - float Ta,Tb,Tc; + float Ta,Tb,Tc; switch(sector){ case 1: Ta = T1 + T2 + T0/2; @@ -369,7 +369,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; } - + // set the voltages in driver driver->setPwm(Ua, Ub, Uc); } @@ -386,7 +386,13 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ float Ts = (now_us - open_loop_timestamp) * 1e-6; // calculate the necessary angle to achieve target velocity - shaft_angle += target_velocity*Ts; + shaft_angle += target_velocity*Ts; + + // check if shaft_angle gets too large + if (shaft_angle >= (_2PI * 4)) + { + shaft_angle = 0; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -403,17 +409,17 @@ void BLDCMotor::angleOpenloop(float target_angle){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; - + // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; else shaft_angle = target_angle; - + // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; -} \ No newline at end of file +} From 74aacac4b6b3d834e32f8ba4a9e6572dbef79e48 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 2 Dec 2020 11:40:15 +0100 Subject: [PATCH 051/749] FEAT added stepper with 2pwm and esp32 refactoring --- .../stepper_driver_2pwm_standalone.ino | 34 +++++ .../stepper_driver_4pwm_standalone.ino | 3 +- keywords.txt | 1 + src/SimpleFOC.h | 1 + src/StepperMotor.cpp | 2 +- src/drivers/StepperDriver2PWM.cpp | 96 ++++++++++++ src/drivers/StepperDriver2PWM.h | 59 ++++++++ src/drivers/hardware_api.h | 23 +++ .../hardware_specific/atmega328_mcu.cpp | 20 +++ src/drivers/hardware_specific/esp32_mcu.cpp | 140 ++++++++++++++---- src/drivers/hardware_specific/generic_mcu.cpp | 17 +++ src/drivers/hardware_specific/stm32_mcu.cpp | 23 +++ 12 files changed, 390 insertions(+), 29 deletions(-) create mode 100644 examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino create mode 100644 src/drivers/StepperDriver2PWM.cpp create mode 100644 src/drivers/StepperDriver2PWM.h diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino new file mode 100644 index 00000000..0e9a0189 --- /dev/null +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -0,0 +1,34 @@ +// Stepper driver standalone example +#include + + +// Stepper driver instance +// StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) +StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); + +void setup() { + + // pwm frequency to be used [Hz] + // for atmega328 fixed to 32kHz + // esp32/stm32/teensy configurable + driver.pwm_frequency = 30000; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + driver.setPwm(3,6); +} \ No newline at end of file diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index 7ac5ac62..e028a737 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -3,6 +3,7 @@ // Stepper driver instance +// StepperDriver4PWM(ph1A, ph1B, ph2A, ph2B, (en1, en2 optional)) StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); void setup() { @@ -10,7 +11,7 @@ void setup() { // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable - driver.pwm_frequency = 50000; + driver.pwm_frequency = 30000; // power supply voltage [V] driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply diff --git a/keywords.txt b/keywords.txt index 6540170d..ba708e5d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -12,6 +12,7 @@ BLDCDriver3PWM KEYWORD1 BLDCDriver6PWM KEYWORD1 BLDCDriver KEYWORD1 StepperDriver4PWM KEYWORD1 +StepperDriver2PWM KEYWORD1 StepperDriver KEYWORD1 initFOC KEYWORD2 diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 41188c20..bb963ef1 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -106,5 +106,6 @@ void loop() { #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" +#include "drivers/StepperDriver2PWM.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index e8e991cf..e7654d86 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -246,7 +246,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){ // calculate the necessary angle to achieve target velocity shaft_angle += target_velocity*Ts; - + // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp new file mode 100644 index 00000000..b6e1c5e1 --- /dev/null +++ b/src/drivers/StepperDriver2PWM.cpp @@ -0,0 +1,96 @@ +#include "StepperDriver2PWM.h" + +StepperDriver2PWM::StepperDriver2PWM(int ph1PWM, int ph1INA, int ph1INB, int ph2PWM, int ph2INA, int ph2INB, int en1, int en2){ + // Pin initialization + pwm1 = ph1PWM; //!< phase 1 pwm pin number + ina1 = ph1INA; //!< phase 1 INA pin number + inb1 = ph1INB; //!< phase 1 INB pin number + pwm2 = ph2PWM; //!< phase 2 pwm pin number + ina2 = ph2INA; //!< phase 2 INA pin number + inb2 = ph2INB; //!< phase 2 INB pin number + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver2PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver2PWM::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +int StepperDriver2PWM::init() { + + // PWM pins + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + pinMode(ina1, OUTPUT); + pinMode(ina2, OUTPUT); + pinMode(inb1, OUTPUT); + pinMode(inb2, OUTPUT); + + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure2PWM(pwm_frequency, pwm1, pwm2); + return 0; +} + + +// Set voltage to the pwm pin +void StepperDriver2PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1(0.0),duty_cycle2(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + // hardware specific writing + duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + // set direction + if( Ualpha > 0 ){ + digitalWrite(inb1, LOW); + digitalWrite(ina1, HIGH); + } + else{ + digitalWrite(ina1, LOW); + digitalWrite(inb1, HIGH); + } + + if( Ubeta > 0 ){ + digitalWrite(ina2, LOW); + digitalWrite(inb2, HIGH); + } + else{ + digitalWrite(inb2, LOW); + digitalWrite(ina2, HIGH); + } + + // write to hardware + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); +} \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h new file mode 100644 index 00000000..ef8797ee --- /dev/null +++ b/src/drivers/StepperDriver2PWM.h @@ -0,0 +1,59 @@ +#ifndef STEPPER_DRIVER_2PWM_h +#define STEPPER_DRIVER_2PWM_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 2 pwm stepper driver class +*/ +class StepperDriver2PWM: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1PWM PWM1 phase pwm pin + @param ph1INA IN1A phase dir pin + @param ph1INB IN1B phase dir pin + @param ph2PWM PWM2 phase pwm pin + @param ph2INA IN2A phase dir pin + @param ph2INB IN2B phase dir pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver2PWM(int ph1PWM,int ph1INA,int ph1INB,int ph2PWM,int ph2INA,int ph2INB, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1; //!< phase 1 pwm pin number + int ina1; //!< phase 1 INA pin number + int inb1; //!< phase 1 INB pin number + int pwm2; //!< phase 2 pwm pin number + int ina2; //!< phase 2 INA pin number + int inb2; //!< phase 2 INB pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index da5ddf51..01c03867 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -4,6 +4,17 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +/** + * 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); + /** * Configuring PWM frequency, resolution and alignment * - BLDC driver - 3PWM setting @@ -47,6 +58,18 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const */ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); +/** + * Function 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); + /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) * - BLDC driver - 3PWM setting diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 58b368f7..24fe563e 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -17,6 +17,18 @@ void _pinHighFrequency(const int pin){ } + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic @@ -29,6 +41,14 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f2daeadf..983d4d54 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -42,7 +42,15 @@ typedef struct { mcpwm_io_signals_t mcpwm_1b; mcpwm_io_signals_t mcpwm_2a; mcpwm_io_signals_t mcpwm_2b; -} stepper_motor_slots_t; +} stepper_4pwm_motor_slots_t; +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; typedef struct { int pinAH; @@ -66,18 +74,26 @@ bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B }; -// define stepper motor slots array -stepper_motor_slots_t esp32_stepper_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; - // define stepper motor slots array bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 }; +// define 4pwm stepper motor slots array +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define 2pwm stepper motor slots array +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B + }; + // configuring high frequency pwm timer // a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller @@ -93,9 +109,9 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm if (dead_zone != NOT_SET){ // dead zone is configured float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2, dead_time/2); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); } _delay(100); @@ -108,15 +124,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil(_MCPWM_FREQ / _PWM_RES_DEF / 2.0 / pwm_frequency) - 1; + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; // constrain prescaler prescaler = _constrain(prescaler, 0, 128); // now calculate the real resolution timer period necessary (pwm resolution) // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = _MCPWM_FREQ / 2.0 / pwm_frequency / (prescaler + 1); + int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); // if pwm resolution too low lower the prescaler if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = _MCPWM_FREQ / 2.0 / pwm_frequency / (--prescaler + 1); + resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler @@ -148,6 +164,51 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].sync.out_sel = 0; } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) 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 + + stepper_2pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; + m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; + break; + } + } + + // disable all the slots with the same MCPWM + // disable 3pwm bldc motor which would go in the same slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -171,14 +232,16 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int } } // disable all the slots with the same MCPWM + // disable 2pwm steppr motor which would go in the same slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; if( slot_num < 2 ){ - // slot 0 of the stepper - esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; // slot 0 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; }else{ // slot 1 of the stepper - esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; } @@ -200,14 +263,14 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int if(!pwm_frequency || pwm_frequency == NOT_SET) 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 - stepper_motor_slots_t m_slot = {}; + stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_motor_slots[slot_num]; + if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; break; } } @@ -216,12 +279,18 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slots 0 and 1 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; // slot 0 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; }else{ // slots 2 and 3 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slots 2 and 3 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; } @@ -236,6 +305,23 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + break; + } + } +} + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic @@ -261,13 +347,13 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ - if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found + if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2b*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); break; } } @@ -300,13 +386,13 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; // slot 0 of the 6pwm bldc - esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; }else{ // slots 2 and 3 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; // slot 1 of the 6pwm bldc - esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; } // configure pins diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index dc322746..d9e11d06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -10,6 +10,14 @@ #else +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + return; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic @@ -35,6 +43,15 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 85d0bd65..942752ee 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -184,6 +184,19 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + // allign the timers + _alignPWMTimers(HT1, HT2, HT2); +} + + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic @@ -211,6 +224,15 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _alignPWMTimers(HT1, HT2, HT3, HT4); } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); +} + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic @@ -221,6 +243,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); } + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic From 00b22fbf885ae884954d3d6a056d2c1fb1dc3831 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 2 Dec 2020 17:53:05 +0100 Subject: [PATCH 052/749] FEAT added true pwm frequency stm + FIX driver init delay --- src/drivers/BLDCDriver3PWM.cpp | 2 ++ src/drivers/BLDCDriver6PWM.cpp | 2 ++ src/drivers/StepperDriver2PWM.cpp | 2 ++ src/drivers/StepperDriver4PWM.cpp | 2 ++ src/drivers/hardware_specific/stm32_mcu.cpp | 30 ++++++++++++++------- 5 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 5932ae3d..a3fc052d 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -35,6 +35,8 @@ void BLDCDriver3PWM::disable() // init hardware pins int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); // PWM pins pinMode(pwmA, OUTPUT); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 76002395..266a5412 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -41,6 +41,8 @@ void BLDCDriver6PWM::disable() // init hardware pins int BLDCDriver6PWM::init() { + // a bit of separation + _delay(1000); // PWM pins pinMode(pwmA_l, OUTPUT); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index b6e1c5e1..72182648 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -41,6 +41,8 @@ void StepperDriver2PWM::disable() // init hardware pins int StepperDriver2PWM::init() { + // a bit of separation + _delay(1000); // PWM pins pinMode(pwm1, OUTPUT); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 5ddaa5d2..28e2dcae 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -39,6 +39,8 @@ void StepperDriver4PWM::disable() // init hardware pins int StepperDriver4PWM::init() { + // a bit of separation + _delay(1000); // PWM pins pinMode(pwm1A, OUTPUT); diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 942752ee..0dc83cf7 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -5,7 +5,8 @@ #define _PWM_RESOLUTION 12 // 12bit #define _PWM_RANGE 4095.0// 2^12 -1 = 4095 -#define _PWM_FREQUENCY 50000 // 50khz +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz #define _HARDWARE_6PWM 1 @@ -188,8 +189,11 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const // - Stepper motor - 2PWM setting // - hardware speciffic void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); // allign the timers @@ -201,8 +205,11 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); @@ -214,8 +221,11 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); @@ -262,8 +272,10 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 100000); // constrain to 100kHz max + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; // find configuration int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); From 03ee308fc07d6db61987a6ee7d7a5bcfa1cb8af6 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 2 Dec 2020 11:21:09 -0600 Subject: [PATCH 053/749] MOT will print if natural_direction is CW --- src/BLDCMotor.cpp | 50 ++++++++++++++++++++++++----------------------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index bed4cd6a..5d36ce35 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void BLDCMotor::init() { if(monitor_port) monitor_port->println("MOT: Initialise variables."); // sanity check for the voltage limit configuration @@ -53,13 +53,13 @@ void BLDCMotor::disable() { // set zero to PWM driver->setPwm(0, 0, 0); - // disable the driver + // disable the driver driver->disable(); } // enable motor driver void BLDCMotor::enable() { - // enable the driver + // enable the driver driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); @@ -93,7 +93,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: Align sensor."); // align the electrical phases of the motor and sensor - // set angle -90 degrees + // set angle -90 degrees float start_angle = shaftAngle(); for (int i = 0; i <=5; i++ ) { @@ -112,6 +112,8 @@ int BLDCMotor::alignSensor() { sensor->natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); } // let the motor stabilize for 2 sec @@ -134,19 +136,19 @@ int BLDCMotor::alignSensor() { } -// Encoder alignment the absolute zero angle +// Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroAlign() { if(monitor_port) monitor_port->println("MOT: Absolute zero align."); // if no absolute zero return if(!sensor->hasAbsoluteZero()) return 0; - + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ - loopFOC(); + loopFOC(); voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); } voltage_q = 0; @@ -167,9 +169,9 @@ int BLDCMotor::absoluteZeroAlign() { // 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 shaft_angle = shaftAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); } @@ -219,7 +221,7 @@ void BLDCMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Space Vector PWM and Sine PWM algorithms -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) @@ -257,7 +259,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { }; // static int trap_150_state = 0; sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - + Ua = Uq + trap_150_map[sector][0] * Uq; Ub = Uq + trap_150_map[sector][1] * Uq; Uc = Uq + trap_150_map[sector][2] * Uq; @@ -272,7 +274,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SinePWM : - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park + Clarke transformation // angle normalization in between 0 and 2pi @@ -299,10 +301,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm // https://www.youtube.com/watch?v=QMSWUMEAejg - // if negative voltages change inverse the phase + // if negative voltages change inverse the phase // angle + 180degrees if(Uq < 0) angle_el += _PI; Uq = abs(Uq); @@ -316,14 +318,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; - // two versions possible + // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage - if (centered) { + if (centered) { T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 - } + } // calculate the duty cycles(times) - float Ta,Tb,Tc; + float Ta,Tb,Tc; switch(sector){ case 1: Ta = T1 + T2 + T0/2; @@ -369,7 +371,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; } - + // set the voltages in driver driver->setPwm(Ua, Ub, Uc); } @@ -386,7 +388,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ float Ts = (now_us - open_loop_timestamp) * 1e-6; // calculate the necessary angle to achieve target velocity - shaft_angle += target_velocity*Ts; + shaft_angle += target_velocity*Ts; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -403,17 +405,17 @@ void BLDCMotor::angleOpenloop(float target_angle){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; - + // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; else shaft_angle = target_angle; - + // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; -} \ No newline at end of file +} From ae696186e3c19dbf919a0ca1f87388bbe1bbadae Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 2 Dec 2020 17:58:09 +0100 Subject: [PATCH 054/749] FEAT added CW to stepper --- src/StepperMotor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index e7654d86..31c81eeb 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -105,6 +105,8 @@ int StepperMotor::alignSensor() { sensor->natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); } // let the motor stabilize for 2 sec From 57f9887982b39f11dba3f1189b0cd9b41179b4ee Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Dec 2020 18:08:04 +0100 Subject: [PATCH 055/749] readme v2.0.1 --- README.md | 9 ++++++++- keywords.txt | 5 ++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index eab04a05..94833c71 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,14 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -

    NEW RELEASE 📢: SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    +> NEW RELEASE 📢: SimpleFOClibrary v2.0.1 +> - ESP32 bugfix +> - frequency setting +> - pwm resolution +> - 2PWM stepper class added `StepperMotor2PWM` +> - some refactoring of examples + +

    SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    ## Arduino *SimpleFOCShield* diff --git a/keywords.txt b/keywords.txt index ba708e5d..c2def3f1 100644 --- a/keywords.txt +++ b/keywords.txt @@ -13,7 +13,9 @@ BLDCDriver6PWM KEYWORD1 BLDCDriver KEYWORD1 StepperDriver4PWM KEYWORD1 StepperDriver2PWM KEYWORD1 -StepperDriver KEYWORD1 +StepperDriver KEYWORD1 +PIDController KEYWORD1 +LowPassFilter KEYWORD1 initFOC KEYWORD2 loopFOC KEYWORD2 @@ -96,6 +98,7 @@ D KEYWORD2 Tf KEYWORD2 voltage_limit KEYWORD2 output_ramp KEYWORD2 +limit KEYWORD2 velocity_limit KEYWORD2 voltage_power_supply KEYWORD2 voltage_sensor_align KEYWORD2 From db61f17ec77e187c3f1ebb4911b2f12019eb6baa Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 7 Dec 2020 17:16:53 +0100 Subject: [PATCH 056/749] atmega2560 intitial support --- src/common/time_utils.cpp | 4 +- .../hardware_specific/atmega2560_mcu.cpp | 158 ++++++++++++++++++ src/drivers/hardware_specific/generic_mcu.cpp | 3 + 3 files changed, 163 insertions(+), 2 deletions(-) create mode 100644 src/drivers/hardware_specific/atmega2560_mcu.cpp diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp index 181d03cf..b00a8c78 100644 --- a/src/common/time_utils.cpp +++ b/src/common/time_utils.cpp @@ -3,7 +3,7 @@ // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) // if arduino uno and other atmega328p chips // use while instad of delay, // due to wrong measurement based on changed timer0 @@ -19,7 +19,7 @@ void _delay(unsigned long ms){ // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) // if arduino uno and other atmega328p chips //return the value based on the prescaler if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp new file mode 100644 index 00000000..5236fb03 --- /dev/null +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -0,0 +1,158 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega2560__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + // https://forum.arduino.cc/index.php?topic=72092.0 + if (pin == 13 || pin == 4 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + else if (pin == 12 || pin == 11 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 10 || pin == 9 ) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 5 || pin == 3 || pin == 2) + TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 8 || pin == 7 || pin == 6) + TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 44 || pin == 45 || pin == 46) + TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +// supports Arudino/ATmega2560 +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index d9e11d06..f0b55be6 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -2,6 +2,9 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 + +#if defined(__AVR_ATmega2560__) // if mcu is not atmega2560 + #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy #elif defined(ESP_H) // or esp32 From 24a578715d68e2a3036db6af558ce32d60e68940 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 7 Dec 2020 17:25:21 +0100 Subject: [PATCH 057/749] atmega328 initial --- src/drivers/hardware_specific/generic_mcu.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index f0b55be6..e450aa88 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -2,8 +2,7 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 - -#if defined(__AVR_ATmega2560__) // if mcu is not atmega2560 +#elif #if defined(__AVR_ATmega2560__) // if mcu is not atmega2560 #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy From 929852d4f6aba50ae9563bbfa558f12144ea6325 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 7 Dec 2020 18:15:06 +0100 Subject: [PATCH 058/749] atmega issuse elif if --- src/drivers/hardware_specific/generic_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index e450aa88..b3641114 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -2,7 +2,7 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 -#elif #if defined(__AVR_ATmega2560__) // if mcu is not atmega2560 +#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy From 3db85670dfc44b49a260fb730ac8d54c7b82d374 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 14 Dec 2020 00:03:52 +0100 Subject: [PATCH 059/749] SimpleFOC with OSC control example 1 --- examples/.gitignore | 1 + .../osc_control_examples/layout1.touchosc | Bin 0 -> 511 bytes .../simplefoc_osc_esp32_3pwm.ino | 182 ++++++++++++++++++ 3 files changed, 183 insertions(+) create mode 100644 examples/.gitignore create mode 100644 examples/osc_control_examples/layout1.touchosc create mode 100644 examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino diff --git a/examples/.gitignore b/examples/.gitignore new file mode 100644 index 00000000..9bb88d37 --- /dev/null +++ b/examples/.gitignore @@ -0,0 +1 @@ +/.DS_Store diff --git a/examples/osc_control_examples/layout1.touchosc b/examples/osc_control_examples/layout1.touchosc new file mode 100644 index 0000000000000000000000000000000000000000..ba4d2afca30fa1bd295d514e07167c165edd5323 GIT binary patch literal 511 zcmWIWW@Zs#;Nak3sATmCWIzI(Kz3$cN@|5(MQ+Z~$-eoA40zi9dp)&Y-1<>f@Pgjt zvKx$?+a5pJwk(9((ChH?_48*>JAW{B`rJq68z&#nfAX+m`;~)jITJSr3fQ&^Oz^Zg z>$@z$tTy*cqK#D6ozJ&opL|t_DDvV`nV;)-Y~g2h?m%|2m5C-L66*~XA9xTH^!U8v z54%9FCM8GTVokmIpAVg!oA_eux&1D_>CHDys%}@#)i|Lu@#CTiSJLG-Hd(%E+S4Iw zv0E>B&&uPct8A`5PPLu)+WoN058c;N)!TF@mUCGNyb;~9dsAL2@6qPgrabwk7Sm|f zoQo+XdFu;vUT$;U9v}F#%&UyCacR=gWfNq3xUXfahd*)KQ#NHDhw#Q3IXerNxLZ4lzWC#CHU8*~n)GScpMBZo zGw&SlUDNfE{WDyy_Vm>;1bDM^1mvEnsb*wgxWvo=Cur~n>=0p6@^ RASp&5GzZd~fW|Q}002w5+d=>U literal 0 HcmV?d00001 diff --git a/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino b/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino new file mode 100644 index 00000000..f296dc01 --- /dev/null +++ b/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino @@ -0,0 +1,182 @@ +/** + * Arduino SimpleFOC + OSC control example + * + * Simple example to show how you can control SimpleFOC via OSC. + * + * It's a nice way to work, easier than changing speeds via Seerial text input. It could also be the basis for + * a functional UI, for example in a lab, art-gallery or similar situation. + * + * For this example we used an ESP32 to run the code, a AS5048B I2C absolute encoder + * and a generic L298 driver board to drive a Emax 4114 gimbal motor. But there is no reason the OSC part will + * not work with any other setup. + * + * You will need: + * + * - a working SimpleFOC setup - motor, driver, encoder + * - a MCU with WiFi and UDP support, for example an ESP32, or an Arduino with Ethernet Shield + * - a device to run an OSC UI + * - a configured OSC UI + * - a WiFi network + * + * To do the OSC UI I used TouchOSC from https://hexler.net/products/touchosc + * There is an example UI file that works with this sketch, see "layout1.touchosc" + * You can open the UI file in 'TouchOSC Editor' (free program) and transfer it to the TouchOSC app on your device. + * + * Alternatively, there are other OSC UIs which may work, e.g. http://opensoundcontrol.org/implementations + * + * Using: + * + * Change the values below to match the WiFi ssid/password of your network. + * Load and run the code on your ESP32. Take a note of the IP address of your ESP32. + * Load and run the UI in TouchOSC. + * Configure TouchOSC to connect to your ESP32. + * The first command you send will cause the ESP32 to start sending responses to your TouchOSC device. + * After this you will see motor position and speed in the UI. + * Have fun controlling your SimpleFOC motors from your smartphone! + * + */ + + +#include "Arduino.h" +#include + +#include +#include + +#include +#include +#include +#include + + +const char ssid[] = "myssid"; // your network SSID (name) +const char pass[] = "mypassword"; // your network password +WiFiUDP Udp; +IPAddress outIp(192,168,1,17); // remote IP (not needed for receive) +const unsigned int outPort = 8000; // remote port (not needed for receive) +const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) + + +OSCErrorCode error; + +MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8); +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); + + +void setup() { + Serial.begin(115200); + + WiFi.begin(ssid, pass); + + Serial.print("Connecting WiFi "); + Serial.println(ssid); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); + } + Udp.begin(inPort); + Serial.println(); + Serial.print("WiFi connected. Local OSC address: "); + Serial.print(WiFi.localIP()); + Serial.print(":"); + Serial.println(inPort); + + delay(2000); + Serial.println("Initializing motor."); + + sensor.init(); + motor.linkSensor(&sensor); + driver.voltage_power_supply = 9; + driver.init(); + motor.linkDriver(&driver); + motor.controller = ControlType::velocity; + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0.001; + motor.PID_velocity.output_ramp = 1000; + motor.LPF_velocity.Tf = 0.01; + motor.voltage_limit = 8; + //motor.P_angle.P = 20; + motor.init(); + motor.initFOC(); + + Serial.println("All initialization complete."); +} + +// velocity set point variable +float target_velocity = 2.0; +// angle set point variable +float target_angle = 1.0; + + +void motorControl(OSCMessage &msg){ + if (msg.isInt(0)) + target_velocity = radians(msg.getInt(0)); + else if (msg.isFloat(0)) + target_velocity = radians(msg.getFloat(0)); + else if (msg.isDouble(0)) + target_velocity = radians(msg.getDouble(0)); + Serial.print("Velocity set to "); + Serial.println(target_velocity); +} + +void cmdControl(OSCMessage &msg){ + char cmdStr[16]; + if (msg.isString(0)) { + msg.getString(0,cmdStr,16); + String it(cmdStr); + motor.command(it); + } +} + +long lastSend = 0; +OSCMessage bundleIN; + +void loop() { + OSCBundle bundleOUT; + + // FOC algorithm function + motor.move(target_velocity); + motor.loopFOC(); + + + int size = Udp.parsePacket(); + if (size > 0) { + while (size--) { + bundleIN.fill(Udp.read()); + } + if (!bundleIN.hasError()) { + bundleIN.dispatch("/mot1/S", motorControl); + bundleIN.dispatch("/mot1/C", cmdControl); + IPAddress ip = Udp.remoteIP(); + if (!( ip==outIp )) { + Serial.print("New connection from "); + Serial.println(ip); + outIp = ip; + } + } + else { + error = bundleIN.getError(); + Serial.print("error: "); + Serial.println(error); + } + bundleIN.empty(); + } + else { // don't receive and send in the same loop... + long now = millis(); + if (now - lastSend > 100) { + int ang = (int)degrees(motor.shaftAngle()) % 360; + if (ang<0) ang = 360-abs(ang); + //BOSCBundle's add' returns the OSCMessage so the message's 'add' can be composed together + bundleOUT.add("/mot1/A").add((int)ang); + bundleOUT.add("/mot1/V").add((int)degrees(motor.shaftVelocity())); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one + lastSend = now; + } + } + +} From 3a71c323df568d982651e3d64114b7d677106148 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Mon, 14 Dec 2020 19:51:25 +0100 Subject: [PATCH 060/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 98e81cb3..71490d53 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, simplefoc_osc_esp32_3pwm From 15cc0b361b837753d6aab289290bdea8365730d2 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 15 Dec 2020 17:56:04 +0100 Subject: [PATCH 061/749] version added v2.0.1 --- README.md | 9 ++++++--- library.properties | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 94833c71..de4554cf 100644 --- a/README.md +++ b/README.md @@ -11,15 +11,18 @@ Therefore this is an attempt to: - Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield -> NEW RELEASE 📢: SimpleFOClibrary v2.0.1 + +> NEW RELEASE 📢: SimpleFOClibrary v2.0.2 +> - Arduino MEGA 2560 support +> - OSC example project + +> SimpleFOClibrary v2.0.1 > - ESP32 bugfix > - frequency setting > - pwm resolution > - 2PWM stepper class added `StepperMotor2PWM` > - some refactoring of examples -

    SimpleFOClibrary v2.0

    • 6PWM support See in docs!
      • Arduino UNO (atmega328)
      • stm32 boards
      • esp32 boards
    • BLDC driver code separated See in docs!
      • BLDC: 6pwm and 3pwm
      • Stepper: 4pwm
      • Hardware specific code in separate files
      • PWM config
    • I2C and SPI sensors multiple busses support by @owennewo See in docs!
    • Hall sensor refactoring @owennewo
    • A lot of refactoring
    Experimental features
    • Initial implementation of Block commutation by @owennewo
      • FOCModulationType::Trapezoid_120
      • FOCModulationType::Trapezoid_150
    • Added support for separate setting of Ud and Uq setting.
      • Preparations for current control
      • Working only for SinePWM modulation at the moment
    - ## Arduino *SimpleFOCShield*

    diff --git a/library.properties b/library.properties index ba0bcaca..40683407 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.0.1 +version=2.0.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 7de09e1afe2c347434e1a412cd0af15ec57cd66d Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Dec 2020 18:56:59 +0100 Subject: [PATCH 062/749] FIX open-loop velocity float bug --- src/BLDCMotor.cpp | 8 +------- src/StepperMotor.cpp | 2 +- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 6e049f34..50ec3180 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -388,13 +388,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ float Ts = (now_us - open_loop_timestamp) * 1e-6; // calculate the necessary angle to achieve target velocity - shaft_angle += target_velocity*Ts; - - // check if shaft_angle gets too large - if (shaft_angle >= (_2PI * 4)) - { - shaft_angle = 0; - } + shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 31c81eeb..f8956da7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -247,7 +247,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){ float Ts = (now_us - open_loop_timestamp) * 1e-6; // calculate the necessary angle to achieve target velocity - shaft_angle += target_velocity*Ts; + shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); From 8d8d996eae9ce820d6337b858ae7830d4019cc82 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Dec 2020 19:00:42 +0100 Subject: [PATCH 063/749] README v2.0.2 --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index de4554cf..e354a07a 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ Therefore this is an attempt to: > NEW RELEASE 📢: SimpleFOClibrary v2.0.2 > - Arduino MEGA 2560 support > - OSC example project +> - floating point bug - open loop velocity > SimpleFOClibrary v2.0.1 > - ESP32 bugfix From ba1ded01383be29266d0f4662261f19c54adcda2 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Dec 2020 19:02:38 +0100 Subject: [PATCH 064/749] v2.0.2 --- keywords.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/keywords.txt b/keywords.txt index c2def3f1..bac2d8af 100644 --- a/keywords.txt +++ b/keywords.txt @@ -115,6 +115,8 @@ DISABLE KEYWORD2 ENABLE KEYWORD2 SpaceVectorPWM KEYWORD2 SinePWM KEYWORD2 +Trapezoid_120 KEYWORD2 +Trapezoid_150 KEYWORD2 pwmA KEYWORD2 pwmB KEYWORD2 From a1b6a690040bf5d59c29d49ee09211b452ba4a62 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 22 Dec 2020 19:06:53 +0100 Subject: [PATCH 065/749] collab links --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e354a07a..922b3581 100644 --- a/README.md +++ b/README.md @@ -14,8 +14,8 @@ Therefore this is an attempt to: > NEW RELEASE 📢: SimpleFOClibrary v2.0.2 > - Arduino MEGA 2560 support -> - OSC example project -> - floating point bug - open loop velocity +> - OSC example project by [@runger1101001](https://github.com/runger1101001) +> - floating point bug - open loop velocity by [@ATILIUS-REGULUS](https://github.com/ATILIUS-REGULUS) > SimpleFOClibrary v2.0.1 > - ESP32 bugfix From 7b7b4ad19d21f2b58bb1fcce1925bbea1c71f48c Mon Sep 17 00:00:00 2001 From: Owen Williams Date: Fri, 8 Jan 2021 16:31:39 +0000 Subject: [PATCH 066/749] fix CCW hallsensor bug + glitch interrupt avoidance --- src/sensors/HallSensor.cpp | 20 +++++++++++++++----- src/sensors/HallSensor.h | 2 +- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2c33a41e..8cf08a0f 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -43,21 +43,31 @@ void HallSensor::handleC() { */ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); - hall_state = C_active + (B_active << 1) + (A_active << 2); + + int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); + + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed + if (new_hall_state == hall_state) { + return; + } + hall_state = new_hall_state; + int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; static Direction old_direction; if (new_electric_sector - electric_sector > 3) { //underflow - direction = static_cast(natural_direction * -1); + direction = Direction::CCW; electric_rotations += direction; } else if (new_electric_sector - electric_sector < (-3)) { //overflow - direction = static_cast(natural_direction); + direction = Direction::CW; electric_rotations += direction; } else { - direction = (new_electric_sector > electric_sector)? static_cast(natural_direction) : static_cast(natural_direction * (-1)); + direction = (new_electric_sector > electric_sector)? Direction::CW : Direction::CCW; } electric_sector = new_electric_sector; + + // glitch avoidance #2 changes in direction can cause velocity spikes. Possible improvements needed in this area if (direction == old_direction) { // not oscilating or just changed direction pulse_diff = new_pulse_timestamp - pulse_timestamp; @@ -97,7 +107,7 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return natural_direction * direction * (_2PI / cpr) / (pulse_diff / 1000000.0); } } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index ad5673e3..5653faf2 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -80,7 +80,7 @@ class HallSensor: public Sensor{ */ int needsAbsoluteZeroSearch() override; - // whether last step was CW (+1) or CCW (-1) direction + // whether last step was CW (+1) or CCW (-1). Note - this is a raw direction (i.e. doesn't include natural_direction reversal) Direction direction; void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); From 8981dc9343e680459121a1ce57dda197bda94e64 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 23 Jan 2021 19:19:31 +0100 Subject: [PATCH 067/749] more info --- README.md | 43 ++++++++++++++++++++----------------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index e354a07a..e9334c1f 100644 --- a/README.md +++ b/README.md @@ -5,26 +5,21 @@ [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) -Proper low-cost and low-power FOC supporting boards are very hard to find today and even may not exist. Even harder to find is a stable and simple FOC algorithm code for BLDC and Stepper motors capable of running on Arduino devices. +We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. +The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. +Additionally, most of the efforts at this moment are still channeled towards the high-power applications of the BLDC motors and proper low-cost and low-power FOC supporting boards are very hard to find today and even may not exist.
    Therefore this is an attempt to: -- Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) -- Develop a modular BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). -- ***New 📢:** Develop a modular Stepper motor board for FOC control:* Arduino StepperFOCShield +- 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) + - Support as many motor + sensor + driver + mcu combinations out there +- 🎯 Develop a modular low-power BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - -> NEW RELEASE 📢: SimpleFOClibrary v2.0.2 +> RELEASE 📢: SimpleFOClibrary v2.0.2 > - Arduino MEGA 2560 support > - OSC example project > - floating point bug - open loop velocity -> SimpleFOClibrary v2.0.1 -> - ESP32 bugfix -> - frequency setting -> - pwm resolution -> - 2PWM stepper class added `StepperMotor2PWM` -> - some refactoring of examples -## Arduino *SimpleFOCShield* +## Arduino *SimpleFOCShield* v2.0.2

    @@ -33,23 +28,25 @@ Therefore this is an attempt to:

    ### Features -- **Plug & play**: In combination with Arduino SimpleFOClibrary -- **Low-cost**: Price of €15 - [Check the pricing](https://www.simplefoc.com/simplefoc_shield_product) -- **Max power 100W** - max current 5A, power-supply 12-24V - - Designed for Gimbal motors with the internal resistance >10 Ω. +- **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 sensor interface**: Integrated 3.3kΩ pullups (configurable) +- **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://www.simplefoc.com/arduino_simplefoc_shield_fabrication), - -##### If you are interested in this board, order your version on this link: [Shop](https://www.simplefoc.com/simplefoc_shield_product) +- **Open Source**: Fully available fabrication files - [how to make it yourself](https://docs.simplefoc.com/arduino_simplefoc_shield_fabrication) -

    +

    -## Arduino *SimpleFOClibrary* +## Arduino *SimpleFOClibrary* v2.0.2

    From ee14e2f94f6864d43c33a3fd5756c9d8dae01cf6 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 29 Jan 2021 23:39:11 +0100 Subject: [PATCH 068/749] support for due --- README.md | 9 +- src/common/foc_utils.h | 3 + src/common/time_utils.cpp | 4 +- .../hardware_specific/atmega328_mcu.cpp | 2 +- src/drivers/hardware_specific/due_mcu.cpp | 391 ++++++++++++++++++ src/drivers/hardware_specific/generic_mcu.cpp | 4 +- src/drivers/hardware_specific/stm32_mcu.cpp | 5 +- src/drivers/hardware_specific/teensy_mcu.cpp | 21 +- 8 files changed, 427 insertions(+), 12 deletions(-) create mode 100644 src/drivers/hardware_specific/due_mcu.cpp diff --git a/README.md b/README.md index e9334c1f..5b7b281d 100644 --- a/README.md +++ b/README.md @@ -13,10 +13,11 @@ Therefore this is an attempt to: - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop a modular low-power BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). -> RELEASE 📢: SimpleFOClibrary v2.0.2 -> - Arduino MEGA 2560 support -> - OSC example project -> - floating point bug - open loop velocity +#### NEXT RELEASE 📢: SimpleFOClibrary v2.0.3 +> #### Implemented features in dev branch +> - Upgrade of the HallSensor implementation by [@owennewo](https://github.com/owennewo) +> - Support for Arduino DUE - everything except the 6PWM mode +> - Support for ATMega328pb ## Arduino *SimpleFOCShield* v2.0.2 diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 7da3f7bc..f82caabd 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -20,8 +20,11 @@ #define _PI_3 1.0471975512 #define _2PI 6.28318530718 #define _3PI_2 4.71238898038 +#define _PI_6 0.52359877559 #define NOT_SET -12345.0 +#define HIGH_IMPEDANCE -1234 +#define HIGH_Z HIGH_IMPEDANCE /** * Function approximating the sine calculation by using fixed size array diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp index b00a8c78..65ebad22 100644 --- a/src/common/time_utils.cpp +++ b/src/common/time_utils.cpp @@ -3,7 +3,7 @@ // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if arduino uno and other atmega328p chips // use while instad of delay, // due to wrong measurement based on changed timer0 @@ -19,7 +19,7 @@ void _delay(unsigned long ms){ // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if arduino uno and other atmega328p chips //return the value based on the prescaler if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 24fe563e..cf12c302 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp new file mode 100644 index 00000000..bb8e6f4b --- /dev/null +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -0,0 +1,391 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(__SAM3X8E__) + +// pwm frequency and max duty cycle +static unsigned long _pwm_frequency; +static int _max_pwm_value = 1023; + +// array mapping the timer values to the interrupt handlers +static IRQn_Type irq_type[] = {TC0_IRQn, TC0_IRQn, TC1_IRQn, TC1_IRQn, TC2_IRQn, TC2_IRQn, TC3_IRQn, TC3_IRQn, TC4_IRQn, TC4_IRQn, TC5_IRQn, TC5_IRQn, TC6_IRQn, TC6_IRQn, TC7_IRQn, TC7_IRQn, TC8_IRQn, TC8_IRQn}; +// current counter values +static volatile uint32_t pwm_counter_vals[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + +// variables copied from wiring_analog.cpp for arduino due +static uint8_t PWMEnabled = 0; +static uint8_t TCChanEnabled[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; +static const uint32_t channelToChNo[] = { 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2 }; +static const uint32_t channelToAB[] = { 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0 }; +static Tc *channelToTC[] = { + TC0, TC0, TC0, TC0, TC0, TC0, + TC1, TC1, TC1, TC1, TC1, TC1, + TC2, TC2, TC2, TC2, TC2, TC2 }; +static const uint32_t channelToId[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 }; + + +// function setting the CMR register +static void TC_SetCMR_ChannelA(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xFFF0FFFF) | v;} +static void TC_SetCMR_ChannelB(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xF0FFFFFF) | v; } + + +// function which starts and syncs the timers +// if the pin is the true PWM pin this function does not do anything +void syncTimers(uint32_t ulPin1,uint32_t ulPin2, uint32_t ulPin3 = -1, uint32_t ulPin4 = -1){ + uint32_t chNo1,chNo2,chNo3,chNo4; + Tc *chTC1 = nullptr,*chTC2 = nullptr,*chTC3 = nullptr,*chTC4 = nullptr; + + // configure timer channel for the first pin if it is a timer pin + uint32_t attr = g_APinDescription[ulPin1].ulPinAttribute; + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { + ETCChannel channel1 = g_APinDescription[ulPin1].ulTCChannel; + chNo1 = channelToChNo[channel1]; + chTC1 = channelToTC[channel1]; + TCChanEnabled[channelToId[channel1]] = 1; + } + + // configure timer channel for the first pin if it is a timer pin + attr = g_APinDescription[ulPin2].ulPinAttribute; + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { + ETCChannel channel2 = g_APinDescription[ulPin2].ulTCChannel; + chNo2 = channelToChNo[channel2]; + chTC2 = channelToTC[channel2]; + TCChanEnabled[channelToId[channel2]] = 1; + } + if(ulPin3 > 0 ){ + // configure timer channel for the first pin if it is a timer pin + attr = g_APinDescription[ulPin3].ulPinAttribute; + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { + ETCChannel channel3 = g_APinDescription[ulPin3].ulTCChannel; + chNo3 = channelToChNo[channel3]; + chTC3 = channelToTC[channel3]; + TCChanEnabled[channelToId[channel3]] = 1; + } + } + if(ulPin4 > 0 ){ + // configure timer channel for the first pin if it is a timer pin + attr = g_APinDescription[ulPin4].ulPinAttribute; + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { + ETCChannel channel4 = g_APinDescription[ulPin4].ulTCChannel; + chNo4 = channelToChNo[channel4]; + chTC4 = channelToTC[channel4]; + TCChanEnabled[channelToId[channel4]] = 1; + } + } + // start timers and make them synced + if(chTC1){ + TC_Start(chTC1, chNo1); + chTC1->TC_BCR = TC_BCR_SYNC; + } + if(chTC2){ + TC_Start(chTC2, chNo2); + chTC2->TC_BCR = TC_BCR_SYNC; + } + if(chTC3 && ulPin3){ + TC_Start(chTC3, chNo3); + chTC3->TC_BCR = TC_BCR_SYNC; + } + if(chTC4 && ulPin4){ + TC_Start(chTC4, chNo4); + chTC4->TC_BCR = TC_BCR_SYNC; + } +} + +// function configuring the pwm frequency for given pin +// possible to supply the pwm pin and the timer pin +void initPWM(uint32_t ulPin, uint32_t pwm_freq){ + // check which pin type + uint32_t attr = g_APinDescription[ulPin].ulPinAttribute; + if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm pin + + if (!PWMEnabled) { + // PWM Startup code + pmc_enable_periph_clk(PWM_INTERFACE_ID); + PWMC_ConfigureClocks(pwm_freq * _max_pwm_value, 0, VARIANT_MCK); + PWMEnabled = 1; + } + + uint32_t chan = g_APinDescription[ulPin].ulPWMChannel; + if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) { + // Setup PWM for this pin + PIO_Configure(g_APinDescription[ulPin].pPort, + g_APinDescription[ulPin].ulPinType, + g_APinDescription[ulPin].ulPin, + g_APinDescription[ulPin].ulPinConfiguration); + PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0); + PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value); + PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0); + PWMC_EnableChannel(PWM_INTERFACE, chan); + g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM; + } + return; + } + + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin + // We use MCLK/2 as clock. + const uint32_t TC = VARIANT_MCK / 2 / pwm_freq; + // Setup Timer for this pin + ETCChannel channel = g_APinDescription[ulPin].ulTCChannel; + uint32_t chNo = channelToChNo[channel]; + uint32_t chA = channelToAB[channel]; + Tc *chTC = channelToTC[channel]; + uint32_t interfaceID = channelToId[channel]; + + if (!TCChanEnabled[interfaceID]) { + pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); + TC_Configure(chTC, chNo, + TC_CMR_TCCLKS_TIMER_CLOCK1 | + TC_CMR_WAVE | // Waveform mode + TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC + TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) + TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | + TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); + TC_SetRC(chTC, chNo, TC); + } + // disable the counter on start + if (chA){ + TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET); + }else{ + TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET); + } + // configure input-ouput structure + if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) { + PIO_Configure(g_APinDescription[ulPin].pPort, + g_APinDescription[ulPin].ulPinType, + g_APinDescription[ulPin].ulPin, + g_APinDescription[ulPin].ulPinConfiguration); + g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM; + } + // enable interrupts + chTC->TC_CHANNEL[chNo].TC_IER = TC_IER_CPAS // interrupt on RA compare match + | TC_IER_CPBS // interrupt on RB compare match + | TC_IER_CPCS; // interrupt on RC compare match + chTC->TC_CHANNEL[chNo].TC_IDR = ~TC_IER_CPAS // interrupt on RA compare match + & ~TC_IER_CPBS // interrupt on RB compare match + & ~ TC_IER_CPCS; // interrupt on RC compare match + // enable interrupts for this timer + NVIC_EnableIRQ(irq_type[channel]); + return; + } +} + +// pwm setting function +// it sets the duty cycle for pwm pin or timer pin +void setPwm(uint32_t ulPin, uint32_t ulValue) { + // check pin type + uint32_t attr = g_APinDescription[ulPin].ulPinAttribute; + if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm + uint32_t chan = g_APinDescription[ulPin].ulPWMChannel; + PWMC_SetDutyCycle(PWM_INTERFACE, chan, ulValue); + return; + } + + if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin + // get the timer variables + ETCChannel channel = g_APinDescription[ulPin].ulTCChannel; + Tc *chTC = channelToTC[channel]; + uint32_t chNo = channelToChNo[channel]; + if(!ulValue) { + // if the value 0 disable counter + if (channelToAB[channel]) + TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR); + else + TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); + }else{ + // if the value not zero + // calculate clock + const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency; + // Map value to Timer ranges 0..max_duty_cycle => 0..TC + // Setup Timer for this pin + ulValue = ulValue * TC; + pwm_counter_vals[channel] = ulValue / _max_pwm_value; + // enable counter + if (channelToAB[channel]) + TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET); + else + TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET); + } + + return; + } +} + +// interrupt handlers for seamless pwm duty-cycle setting +void TC0_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC0, 0); + // update the counters + if(pwm_counter_vals[0]) TC_SetRA(TC0, 0, pwm_counter_vals[0]); + if(pwm_counter_vals[1]) TC_SetRB(TC0, 0, pwm_counter_vals[1]); +} + +void TC1_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC0, 1); + // update the counters + if(pwm_counter_vals[2]) TC_SetRA(TC0, 1, pwm_counter_vals[2]); + if(pwm_counter_vals[3]) TC_SetRB(TC0, 1, pwm_counter_vals[3]); +} + +void TC2_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC0, 2); + // update the counters + if(pwm_counter_vals[4]) TC_SetRA(TC0, 2, pwm_counter_vals[4]); + if(pwm_counter_vals[5]) TC_SetRB(TC0, 2, pwm_counter_vals[5]); +} +void TC3_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC1, 0); + // update the counters + if(pwm_counter_vals[6]) TC_SetRA(TC1, 0, pwm_counter_vals[6]); + if(pwm_counter_vals[7]) TC_SetRB(TC1, 0, pwm_counter_vals[7]); +} + +void TC4_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC1, 1); + // update the counters + if(pwm_counter_vals[8]) TC_SetRA(TC1, 1, pwm_counter_vals[8]); + if(pwm_counter_vals[9]) TC_SetRB(TC1, 1, pwm_counter_vals[9]); +} + +void TC5_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC1, 2); + // update the counters + if(pwm_counter_vals[10]) TC_SetRA(TC1, 2, pwm_counter_vals[10]); + if(pwm_counter_vals[11]) TC_SetRB(TC1, 2, pwm_counter_vals[11]); +} +void TC6_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC2, 0); + // update the counters + if(pwm_counter_vals[12]) TC_SetRA(TC2, 0, pwm_counter_vals[12]); + if(pwm_counter_vals[13]) TC_SetRB(TC2, 0, pwm_counter_vals[13]); +} + +void TC7_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC2, 1); + // update the counters + if(pwm_counter_vals[14]) TC_SetRA(TC2, 1, pwm_counter_vals[14]); + if(pwm_counter_vals[15]) TC_SetRB(TC2, 1, pwm_counter_vals[15]); +} + +void TC8_Handler() +{ + // read/clear interrupt status + TC_GetStatus(TC2, 2); + // update the counters + if(pwm_counter_vals[16]) TC_SetRA(TC2, 2, pwm_counter_vals[16]); + if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]); +} + +// implementation of the hardware_api.cpp +// --------------------------------------------------------------------------------------------------------------------------------- + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware specific +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + // save the pwm frequency + _pwm_frequency = pwm_frequency; + // cinfigure pwm pins + initPWM(pinA, _pwm_frequency); + initPWM(pinB, _pwm_frequency); + initPWM(pinC, _pwm_frequency); + // sync the timers if possible + syncTimers(pinA, pinB, pinC); +} + + +// Configuring PWM frequency, resolution and alignment +//- Stepper driver - 2PWM setting +// - hardware specific +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + // save the pwm frequency + _pwm_frequency = pwm_frequency; + // cinfigure pwm pins + initPWM(pinA, _pwm_frequency); + initPWM(pinB, _pwm_frequency); + // sync the timers if possible + syncTimers(pinA, pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + // save the pwm frequency + _pwm_frequency = pwm_frequency; + // cinfigure pwm pins + initPWM(pinA, _pwm_frequency); + initPWM(pinB, _pwm_frequency); + initPWM(pinC, _pwm_frequency); + initPWM(pinD, _pwm_frequency); + // sync the timers if possible + syncTimers(pinA, pinB, pinC, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,_max_pwm_value] + setPwm(pinA, _max_pwm_value*dc_a); + setPwm(pinB, _max_pwm_value*dc_b); + setPwm(pinC, _max_pwm_value*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,_max_pwm_value] + setPwm(pin1A, _max_pwm_value*dc_1a); + setPwm(pin1B, _max_pwm_value*dc_1b); + setPwm(pin2A, _max_pwm_value*dc_2a); + setPwm(pin2B, _max_pwm_value*dc_2b); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - Stepper driver - 2PWM setting +// - hardware specific +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,_max_pwm_value] + setPwm(pinA, _max_pwm_value*dc_a); + setPwm(pinB, _max_pwm_value*dc_b); +} + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index b3641114..d55b9ff3 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,11 +1,13 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328 #elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy +#elif defined(__arm__) && defined(__SAM3X8E__) // or due + #elif defined(ESP_H) // or esp32 #elif defined(_STM32_DEF_) // or stm32 diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 0dc83cf7..c6298d1b 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -2,13 +2,13 @@ #include "../hardware_api.h" #if defined(_STM32_DEF_) - +// default pwm parameters #define _PWM_RESOLUTION 12 // 12bit #define _PWM_RANGE 4095.0// 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz - +// 6pwm parameters #define _HARDWARE_6PWM 1 #define _SOFTWARE_6PWM 0 #define _ERROR_6PWM -1 @@ -118,6 +118,7 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha HT4->resume(); } +// configure hardware 6pwm interface only one timer with inverted channels HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { PinName uhPinName = digitalPinToPinName(pinA_h); diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 10bd24c5..2a584876 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -9,16 +9,25 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); - if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor } // function setting the high pwm frequency to the supplied pins @@ -33,6 +42,14 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _setHighFrequency(pwm_frequency, pinD); } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic From f32cf273a318e1657ec0f7d983c0e2d4d819b55c Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 1 Feb 2021 21:53:54 +0100 Subject: [PATCH 069/749] initial implementation of the current control for inline sensing --- README.md | 2 +- keywords.txt | 18 ++- src/BLDCMotor.cpp | 115 +++++++++++------- src/BLDCMotor.h | 7 +- src/SimpleFOC.h | 1 + src/StepperMotor.cpp | 14 +-- src/common/base_classes/BLDCDriver.h | 5 + src/common/base_classes/CurrentSense.h | 20 +++ src/common/base_classes/FOCMotor.cpp | 29 +++-- src/common/base_classes/FOCMotor.h | 37 ++++-- src/common/defaults.h | 28 ++++- src/common/foc_utils.h | 20 +++ src/common/pid.cpp | 4 +- src/common/pid.h | 2 +- src/current_sense/InlineCurrentSense.cpp | 63 ++++++++++ src/current_sense/InlineCurrentSense.h | 47 +++++++ src/current_sense/hardware_api.h | 11 ++ .../hardware_specific/generic_mcu.cpp | 29 +++++ src/drivers/BLDCDriver3PWM.cpp | 36 ++++-- src/drivers/BLDCDriver3PWM.h | 10 +- src/drivers/BLDCDriver6PWM.cpp | 7 +- 21 files changed, 417 insertions(+), 88 deletions(-) create mode 100644 src/common/base_classes/CurrentSense.h create mode 100644 src/current_sense/InlineCurrentSense.cpp create mode 100644 src/current_sense/InlineCurrentSense.h create mode 100644 src/current_sense/hardware_api.h create mode 100644 src/current_sense/hardware_specific/generic_mcu.cpp diff --git a/README.md b/README.md index 5b7b281d..b5cb13b9 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ Therefore this is an attempt to: - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop a modular low-power BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). -#### NEXT RELEASE 📢: SimpleFOClibrary v2.0.3 +##### NEXT RELEASE 📢: SimpleFOClibrary v2.0.3 > #### Implemented features in dev branch > - Upgrade of the HallSensor implementation by [@owennewo](https://github.com/owennewo) > - Support for Arduino DUE - everything except the 6PWM mode diff --git a/keywords.txt b/keywords.txt index bac2d8af..c864e5a3 100644 --- a/keywords.txt +++ b/keywords.txt @@ -15,7 +15,10 @@ StepperDriver4PWM KEYWORD1 StepperDriver2PWM KEYWORD1 StepperDriver KEYWORD1 PIDController KEYWORD1 -LowPassFilter KEYWORD1 +LowPassFilter KEYWORD1 +InlineCurrentSense KEYWORD1 +CurrentSense KEYWORD1 + initFOC KEYWORD2 loopFOC KEYWORD2 @@ -44,9 +47,13 @@ Pullup KEYWORD1 Direction KEYWORD1 MagneticSensorI2CConfig_s KEYWORD1 MagneticSensorSPIConfig_s KEYWORD1 +DQVoltage_s KEYWORD1 +DQCurrent_s KEYWORD1 +PhaseCurrent_s KEYWORD1 linkDriver KEYWORD2 linkSensor KEYWORD2 +linkCurrentSense KEYWORD2 handleA KEYWORD2 handleB KEYWORD2 handleIndex KEYWORD2 @@ -63,9 +70,13 @@ needsAbsoluteZeroSearch KEYWORD2 useMonitoring KEYWORD2 angleOpenloop KEYWORD2 velocityOpenloop KEYWORD2 +getCurrents KEYWORD2 +getFOCCurrents KEYWORD2 voltage_q KEYWORD2 voltage_d KEYWORD2 +current KEYWORD2 +current_measured KEYWORD2 shaft_angle_sp KEYWORD2 shaft_velocity_sp KEYWORD2 shaft_angle KEYWORD2 @@ -84,6 +95,7 @@ velocity KEYWORD2 velocity_openloop KEYWORD2 angle KEYWORD2 angle_openloop KEYWORD2 +torque KEYWORD2 ON KEYWORD2 @@ -134,6 +146,9 @@ enable_pin KEYWORD2 enable_pin1 KEYWORD2 enable_pin2 KEYWORD2 pole_pairs KEYWORD2 +dc_a KEYWORD2 +dc_b KEYWORD2 +dc_c KEYWORD2 DEF_POWER_SUPPLY LITERAL1 DEF_PID_VEL_P LITERAL1 @@ -150,6 +165,7 @@ _SQRT3_2 LITERAL1 _SQRT2 LITERAL1 _120_D2R LITERAL1 _PI_2 LITERAL1 +_PI_6 LITERAL1 _2PI LITERAL1 _3PI_2 LITERAL1 _PI_3 LITERAL1 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 50ec3180..149897fe 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -10,15 +10,15 @@ // } // BLDCMotor( int pp) -// - phA, phB, phC - motor A,B,C phase pwm pins // - pp - pole pair number -// - cpr - counts per rotation number (cpm=ppm*4) -// - enable pin - (optional input) -BLDCMotor::BLDCMotor(int pp) +// - R - motor phase resistance +BLDCMotor::BLDCMotor(int pp, float _R) : FOCMotor() { // save pole pairs number pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; } @@ -32,12 +32,28 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { if(monitor_port) monitor_port->println("MOT: Initialise variables."); + + // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit + if( !current_sense && phase_resistance != NOT_SET ) { + float new_voltage_limit = current_limit / (phase_resistance*1.5); // v_lim = c_lim / (3/2 phase resistance) - worst case + // use it if it is less then voltage_limit set by the user + voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; + } // sanity check for the voltage limit configuration if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits - PID_velocity.limit = voltage_limit; + if(current_sense){ + // current control loop controls voltage + PID_current_q.limit = voltage_limit; + PID_current_d.limit = voltage_limit; + // velocity control loop controls current + PID_velocity.limit = current_limit; + }else{ + PID_velocity.limit = voltage_limit; + } P_angle.limit = velocity_limit; _delay(500); @@ -96,16 +112,16 @@ int BLDCMotor::alignSensor() { // set angle -90 degrees float start_angle = shaftAngle(); - for (int i = 0; i <=5; i++ ) { - float angle = _3PI_2 + _2PI * i / 6.0; + for (int i = 0; i <=50; i++ ) { + float angle = _3PI_2 + _2PI * i / 51.0; setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(200); + _delay(20); } float mid_angle = shaftAngle(); - for (int i = 5; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 6.0; + for (int i = 50; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 51.0; setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(200); + _delay(20); } if (mid_angle < start_angle) { if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); @@ -149,9 +165,9 @@ int BLDCMotor::absoluteZeroAlign() { // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ loopFOC(); - voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + voltage.q = PID_velocity(velocity_index_search - shaftVelocity()); } - voltage_q = 0; + voltage.q = 0; // disable motor setPhaseVoltage(0, 0, 0); @@ -171,8 +187,23 @@ int BLDCMotor::absoluteZeroAlign() { void BLDCMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); + electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs)); + + // if current sense linked used true foc architecture + // regulate the current in d axis to be equal to 0 + if(current_sense && controller != ControlType::voltage){ // + // read dq currents + current_measured = current_sense->getFOCCurrents(electrical_angle); + // filter values + current_measured.q = LPF_current_q(current_measured.q); + current_measured.d = LPF_current_d(current_measured.d); + // calculate the phase voltages + voltage.q = PID_current_q(current.q - current_measured.q); + voltage.d = PID_current_d(-current_measured.d); + } + // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } // Iterative function running outer loop of the FOC algorithm @@ -187,21 +218,25 @@ void BLDCMotor::move(float new_target) { shaft_velocity = shaftVelocity(); // choose control loop switch (controller) { + case ControlType::torque: case ControlType::voltage: - voltage_q = target; + if(current_sense) current.q = target; + else voltage.q = target; break; case ControlType::angle: // angle set point // include angle loop shaft_angle_sp = target; shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + if(current_sense) current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + else voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); break; case ControlType::velocity: // velocity set point // include velocity loop shaft_velocity_sp = target; - voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + if(current_sense) current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + else voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); break; case ControlType::velocity_openloop: // velocity control in open loop @@ -228,6 +263,7 @@ void BLDCMotor::move(float new_target) { void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { const bool centered = true; + float center; int sector; float _ca,_sa; @@ -236,40 +272,35 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { case FOCModulationType::Trapezoid_120 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 static int trap_120_map[6][3] = { - {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + {HIGH_IMPEDANCE,1,-1},{-1,1,HIGH_IMPEDANCE},{-1,HIGH_IMPEDANCE,1},{HIGH_IMPEDANCE,-1,1},{1,-1,HIGH_IMPEDANCE},{1,HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z }; // static int trap_120_state = 0; - sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - - Ua = Uq + trap_120_map[sector][0] * Uq; - Ub = Uq + trap_120_map[sector][1] * Uq; - Uc = Uq + trap_120_map[sector][2] * Uq; - - if (centered) { - Ua += (driver->voltage_limit)/2 -Uq; - Ub += (driver->voltage_limit)/2 -Uq; - Uc += (driver->voltage_limit)/2 -Uq; - } + sector = 6 * (_normalizeAngle(angle_el + _PI_6 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + // centering the voltages around either + // centered == true > driver.volage_limit/2 + // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + center = centered ? (driver->voltage_limit)/2 : Uq; + + Ua = trap_120_map[sector][0] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][0] * Uq + center; + Ub = trap_120_map[sector][1] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][1] * Uq + center; + Uc = trap_120_map[sector][2] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][2] * Uq + center; break; case FOCModulationType::Trapezoid_150 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 static int trap_150_map[12][3] = { - {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + {HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,HIGH_IMPEDANCE},{-1,1,1},{-1,HIGH_IMPEDANCE,1},{-1,-1,1},{HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,HIGH_IMPEDANCE},{1,-1,-1},{1,HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z }; // static int trap_150_state = 0; - sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes - - Ua = Uq + trap_150_map[sector][0] * Uq; - Ub = Uq + trap_150_map[sector][1] * Uq; - Uc = Uq + trap_150_map[sector][2] * Uq; - - //center - if (centered) { - Ua += (driver->voltage_limit)/2 -Uq; - Ub += (driver->voltage_limit)/2 -Uq; - Uc += (driver->voltage_limit)/2 -Uq; - } + sector = 12 * (_normalizeAngle(angle_el + _PI_6 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + // centering the voltages around either + // centered == true > driver.volage_limit/2 + // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + center = centered ? (driver->voltage_limit)/2 : Uq; + + Ua = ( trap_150_map[sector][0] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][0] * Uq + center; + Ub = ( trap_150_map[sector][1] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][1] * Uq + center; + Uc = ( trap_150_map[sector][2] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][2] * Uq + center; break; diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 164cc108..147b6456 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -18,8 +18,9 @@ class BLDCMotor: public FOCMotor /** BLDCMotor class constructor @param pp pole pairs number + @param R motor phase resistance */ - BLDCMotor(int pp); + BLDCMotor(int pp, float R = NOT_SET); /** * Function linking a motor and a foc driver @@ -65,8 +66,8 @@ class BLDCMotor: public FOCMotor */ void move(float target = NOT_SET) override; - float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform private: diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index bb963ef1..82d3e375 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -107,5 +107,6 @@ void loop() { #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" +#include "current_sense/InlineCurrentSense.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index f8956da7..777f85ae 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -142,10 +142,10 @@ int StepperMotor::absoluteZeroAlign() { // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ loopFOC(); - voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + voltage.q = PID_velocity(velocity_index_search - shaftVelocity()); } - voltage_q = 0; - voltage_d = 0; + voltage.q = 0; + voltage.d = 0; // disable motor setPhaseVoltage(0, 0, 0); @@ -166,7 +166,7 @@ void StepperMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(voltage.q, voltage.d, _electricalAngle(shaft_angle, pole_pairs)); } // Iterative function running outer loop of the FOC algorithm @@ -182,20 +182,20 @@ void StepperMotor::move(float new_target) { // choose control loop switch (controller) { case ControlType::voltage: - voltage_q = target; + voltage.q = target; break; case ControlType::angle: // angle set point // include angle loop shaft_angle_sp = target; shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); break; case ControlType::velocity: // velocity set point // include velocity loop shaft_velocity_sp = target; - voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); break; case ControlType::velocity_openloop: // velocity control in open loop diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 708323fb..44ed5786 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -14,6 +14,11 @@ class BLDCDriver{ long pwm_frequency; //!< pwm frequency value in hertz float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor + + + float dc_a; //!< currently set duty cycle on phaseA + float dc_b; //!< currently set duty cycle on phaseB + float dc_c; //!< currently set duty cycle on phaseC /** * Set phase voltages to the harware diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h new file mode 100644 index 00000000..6f2c43ab --- /dev/null +++ b/src/common/base_classes/CurrentSense.h @@ -0,0 +1,20 @@ +#ifndef CURRENTSENSE_H +#define CURRENTSENSE_H + + +#include "../foc_utils.h" + +/** + * Current sensing abstract class defintion + * Each current sensoring implementation needs to extend this interface + */ +class CurrentSense{ + public: + + virtual void init(); + virtual PhaseCurrent_s getCurrents(); + virtual DQCurrent_s getFOCCurrents(float angle_el); + +}; + +#endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 1d1a0f43..02ee325f 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -9,6 +9,8 @@ FOCMotor::FOCMotor() velocity_limit = DEF_VEL_LIM; // maximum voltage to be set to the motor voltage_limit = DEF_POWER_SUPPLY; + // not set on the begining + current_limit = DEF_CURRENT_LIM; // index search velocity velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; @@ -23,22 +25,35 @@ FOCMotor::FOCMotor() // default target value target = 0; - voltage_d = 0; - voltage_q = 0; + voltage.d = 0; + voltage.q = 0; + // current target values + current.d = 0; + current.q = 0; //monitor_port monitor_port = nullptr; //sensor sensor = nullptr; + //current sensor + current_sense = nullptr; } /** - Sensor communication methods + Sensor linking method */ void FOCMotor::linkSensor(Sensor* _sensor) { sensor = _sensor; } + +/** + CurrentSense linking method +*/ +void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) { + current_sense = _current_sense; +} + // shaft angle calculation float FOCMotor::shaftAngle() { // if no sensor linked return 0 @@ -67,7 +82,7 @@ void FOCMotor::monitor() { switch (controller) { case ControlType::velocity_openloop: case ControlType::velocity: - monitor_port->print(voltage_q); + monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_velocity_sp); monitor_port->print("\t"); @@ -75,14 +90,14 @@ void FOCMotor::monitor() { break; case ControlType::angle_openloop: case ControlType::angle: - monitor_port->print(voltage_q); + monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_angle_sp); monitor_port->print("\t"); monitor_port->println(shaft_angle); break; case ControlType::voltage: - monitor_port->print(voltage_q); + monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_angle); monitor_port->print("\t"); @@ -213,7 +228,7 @@ int FOCMotor::command(String user_command) { switch((int)value){ case 0: // get voltage if(monitor_port) monitor_port->print("Uq: "); - if(monitor_port) monitor_port->println(voltage_q); + if(monitor_port) monitor_port->println(voltage.q); break; case 1: // get velocity if(monitor_port) monitor_port->print("Velocity: "); diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 986e1ab2..4ec6532c 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -3,7 +3,7 @@ #include "Arduino.h" #include "Sensor.h" -#include "BLDCDriver.h" +#include "CurrentSense.h" #include "../time_utils.h" #include "../foc_utils.h" @@ -17,6 +17,7 @@ */ enum ControlType{ voltage,//!< Torque control using voltage + torque,//!< Torque control using current velocity,//!< Velocity motion control angle,//!< Position/angle motion control velocity_openloop, @@ -58,6 +59,13 @@ class FOCMotor */ void linkSensor(Sensor* sensor); + /** + * Function linking a motor and current sensing + * + * @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements + */ + void linkCurrentSense(CurrentSense* current_sense); + /** * Function initializing FOC algorithm @@ -99,19 +107,25 @@ class FOCMotor // state variables float target; //!< current target value - depends of the controller float shaft_angle;//!< current motor angle + float electrical_angle;//!< current electrical angle float shaft_velocity;//!< current motor velocity float shaft_velocity_sp;//!< current target velocity float shaft_angle_sp;//!< current target angle - float voltage_q;//!< current voltage u_q set - float voltage_d;//!< current voltage u_d set + DQVoltage_s voltage;//!< current d and q voltage set to the motor + DQCurrent_s current;//!< current d and q current set to the motor + DQCurrent_s current_measured;//!< current d and q current measured // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter float velocity_index_search;//!< target velocity for index search - int pole_pairs;//!< Motor pole pairs number + + // motor physical parameters + float phase_resistance; //!< motor phase resistance + int pole_pairs;//!< motor pole pairs number // limiting variables float voltage_limit; //!< Voltage limitting variable - global limit + float current_limit; //!< Current limitting variable - global limit float velocity_limit; //!< Velocity limitting variable - global limit float zero_electric_angle;//! Date: Mon, 1 Feb 2021 22:17:51 +0100 Subject: [PATCH 070/749] added one true foc example --- .../true_foc_control/true_foc_control.ino | 126 ++++++++++++++++++ keywords.txt | 5 + 2 files changed, 131 insertions(+) create mode 100644 examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino diff --git a/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino b/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino new file mode 100644 index 00000000..0ed17c89 --- /dev/null +++ b/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino @@ -0,0 +1,126 @@ +/** + + Torque control example using current control loop. + + This example is based on the Arduino SimpleFOCShield V2.0 + The current is measured inline with the shunt resistor or R= 0.01 Ohm, and amplification gain og G = 50 +*/ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 11, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// current sensor +// InlineCurrentSense(shunt_resistor, gain, pinA, pinB, (optional pinC)) +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.pwm_frequency = 50000; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set motion control loop to be used + // ControlType::torque, ControlType::velocity or ControlType::angle + motor.controller = ControlType::velocity; + + // foc currnet control parameters (all default) + motor.PID_current_q.P = 5; + motor.PID_current_q.I= 1000; + motor.PID_current_d.P= 5; + motor.PID_current_d.I = 1000; + motor.LPF_current_q.Tf = 0.002; // 1ms default + motor.LPF_current_d.Tf = 0.002; // 1ms default + + // motion control parameters + motor.PID_velocity.P = 0.05; + motor.PID_velocity.I = 1; + motor.PID_velocity.D = 0; + motor.LPF_velocity.Tf = 0.005; + motor.current_limit = 0.5; // 0.5 Amps limit + + _delay(1000); + // initialise the current sensing + current_sense.init(); + // linking the current sensing + motor.linkCurrentSense(¤t_sense); + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // use monitoring with serial + Serial.begin(250000); + + Serial.println("Motor ready."); +} + + +// target voltage to be set to the motor +float target_angle = 0; + +// counting variable for user display +long t = 0; + + void loop() { + + // set the phase voltages Ud and Uq + motor.loopFOC(); + motor.move(target_angle); + + // show user the currents + if (!t) { + Serial.print(motor.current_measured.q*1000); // mAmps + Serial.print("\t"); + Serial.println(motor.current_measured.d*1000); // mAmps + } + t > 20 ? t = 0 : t++; + + // communicate with the user + serialReceiveUserCommand(); +} + + +// utility function enabling serial communication with the user to set the target values +// this function can be implemented in serialEvent function as well +void serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + + // change the motor target + target_angle = received_chars.toFloat(); + Serial.print("Target angle [radian]: "); + Serial.println(target_angle); + + // reset the command buffer + received_chars = ""; + } + } +} \ No newline at end of file diff --git a/keywords.txt b/keywords.txt index c864e5a3..f33a6b1d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -37,7 +37,11 @@ monitor KEYWORD3 command KEYWORD3 PID_velocity KEYWORD2 +PID_current_q KEYWORD2 +PID_current_d KEYWORD2 LPF_velocity KEYWORD2 +LPF_current_q KEYWORD2 +LPF_current_d KEYWORD2 P_angle KEYWORD2 ControlType KEYWORD1 @@ -109,6 +113,7 @@ I KEYWORD2 D KEYWORD2 Tf KEYWORD2 voltage_limit KEYWORD2 +current_limit KEYWORD2 output_ramp KEYWORD2 limit KEYWORD2 velocity_limit KEYWORD2 From 2f502946b22ca2f7397e3575a38def1bcc5f9887 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 3 Feb 2021 17:24:07 +0100 Subject: [PATCH 071/749] separated torque control voltage/current/foc --- examples/.gitignore | 1 - keywords.txt | 28 ++- src/BLDCMotor.cpp | 160 +++++++++++------- src/StepperMotor.cpp | 60 ++++--- src/common/base_classes/CurrentSense.cpp | 29 ++++ src/common/base_classes/CurrentSense.h | 32 +++- src/common/base_classes/FOCMotor.cpp | 32 ++-- src/common/base_classes/FOCMotor.h | 21 ++- src/common/foc_utils.cpp | 15 +- src/common/foc_utils.h | 13 +- src/current_sense/InlineCurrentSense.cpp | 54 +++--- src/current_sense/InlineCurrentSense.h | 29 +++- src/current_sense/hardware_api.h | 5 +- .../hardware_specific/generic_mcu.cpp | 1 + src/drivers/BLDCDriver3PWM.cpp | 14 +- 15 files changed, 340 insertions(+), 154 deletions(-) delete mode 100644 examples/.gitignore create mode 100644 src/common/base_classes/CurrentSense.cpp diff --git a/examples/.gitignore b/examples/.gitignore deleted file mode 100644 index 9bb88d37..00000000 --- a/examples/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/.DS_Store diff --git a/keywords.txt b/keywords.txt index f33a6b1d..dccfed1b 100644 --- a/keywords.txt +++ b/keywords.txt @@ -25,6 +25,7 @@ loopFOC KEYWORD2 disable KEYWORD2 _delay KEYWORD3 +_sqrt KEYWORD3 _micros KEYWORD3 _sin KEYWORD3 _cos KEYWORD3 @@ -44,7 +45,8 @@ LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 -ControlType KEYWORD1 +MotionControlType KEYWORD1 +TorqueControlType KEYWORD1 FOCModulationType KEYWORD2 Quadrature KEYWORD1 Pullup KEYWORD1 @@ -76,15 +78,18 @@ angleOpenloop KEYWORD2 velocityOpenloop KEYWORD2 getCurrents KEYWORD2 getFOCCurrents KEYWORD2 +getCurrent KEYWORD2 voltage_q KEYWORD2 voltage_d KEYWORD2 current KEYWORD2 current_measured KEYWORD2 shaft_angle_sp KEYWORD2 +electrical_angle KEYWORD2 shaft_velocity_sp KEYWORD2 shaft_angle KEYWORD2 shaft_velocity KEYWORD2 +torque_controller KEYWORD2 controller KEYWORD2 pullup KEYWORD2 quadrature KEYWORD2 @@ -92,6 +97,9 @@ foc_modulation KEYWORD2 target KEYWORD2 pwm_frequency KEYWORD2 dead_zone KEYWORD2 +gain_adjust_a KEYWORD2 +gain_adjust_b KEYWORD2 +gain_adjust_c KEYWORD2 voltage KEYWORD2 @@ -100,6 +108,8 @@ velocity_openloop KEYWORD2 angle KEYWORD2 angle_openloop KEYWORD2 torque KEYWORD2 +current KEYWORD2 +foc_current KEYWORD2 ON KEYWORD2 @@ -158,12 +168,20 @@ dc_c KEYWORD2 DEF_POWER_SUPPLY LITERAL1 DEF_PID_VEL_P LITERAL1 DEF_PID_VEL_I LITERAL1 -DEF_PID_VEL_U_RAMP LITERAL1 +DEF_PID_VEL_D LITERAL1 +DEF_PID_VEL_RAMP LITERAL1 DEF_P_ANGLE_P LITERAL1 -DEF_VEL_LIM LITERAL1 +DEF_PID_VEL_LIMIT LITERAL1 DEF_INDEX_SEARCH_TARGET_VELOCITY LITERAL1 DEF_VOLTAGE_SENSOR_ALIGN LITERAL1 DEF_VEL_FILTER_Tf LITERAL1 +DEF_CURRENT_LIM LITERAL1 +DEF_CURR_FILTER_Tf LITERAL1 +DEF_PID_CURR_LIMIT LITERAL1 +DEF_PID_CURR_RAMP LITERAL1 +DEF_PID_CURR_D LITERAL1 +DEF_PID_CURR_I LITERAL1 +DEF_PID_CURR_P LITERAL1 _2_SQRT3 LITERAL1 _1_SQRT3 LITERAL1 _SQRT3_2 LITERAL1 @@ -175,4 +193,6 @@ _2PI LITERAL1 _3PI_2 LITERAL1 _PI_3 LITERAL1 _SQRT3 LITERAL1 -_PI LITERAL1 \ No newline at end of file +_PI LITERAL1 +_HIGH_Z LITERAL1 +_HIGH_IMPEDANCE LITERAL1 \ No newline at end of file diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 149897fe..574ef63b 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,14 +1,5 @@ #include "BLDCMotor.h" -// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) -// // - phA, phB, phC - motor A,B,C phase pwm pins -// // - pp - pole pair number -// // - cpr - counts per rotation number (cpm=ppm*4) -// // - enable pin - (optional input) -// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ -// noop; -// } - // BLDCMotor( int pp) // - pp - pole pair number // - R - motor phase resistance @@ -35,7 +26,7 @@ void BLDCMotor::init() { // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && phase_resistance != NOT_SET ) { - float new_voltage_limit = current_limit / (phase_resistance*1.5); // v_lim = c_lim / (3/2 phase resistance) - worst case + float new_voltage_limit = current_limit / (phase_resistance*1.5); // v_lim = current_lim / (3/2 phase resistance) - worst case // use it if it is less then voltage_limit set by the user voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; } @@ -71,6 +62,8 @@ void BLDCMotor::disable() driver->setPwm(0, 0, 0); // disable the driver driver->disable(); + // motor status update + enabled = 0; } // enable motor driver void BLDCMotor::enable() @@ -79,7 +72,8 @@ void BLDCMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); - + // motor status update + enabled = 1; } /** @@ -109,20 +103,25 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: Align sensor."); // align the electrical phases of the motor and sensor - // set angle -90 degrees - + // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); - for (int i = 0; i <=50; i++ ) { - float angle = _3PI_2 + _2PI * i / 51.0; + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + // move one electrical revolution forward + _delay(500); + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0; setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(20); + _delay(2); } + // take and angle in the middle float mid_angle = shaftAngle(); - for (int i = 50; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 51.0; + // move one electrical revolution forward + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0 ; setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(20); + _delay(2); } + // determin the direction the sensor moved if (mid_angle < start_angle) { if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); sensor->natural_direction = Direction::CCW; @@ -132,8 +131,8 @@ int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); } - // let the motor stabilize for 2 sec - _delay(2000); + // let the motor stabilize for1 sec + _delay(1000); // set sensor to zero sensor->initRelativeZero(); _delay(500); @@ -185,73 +184,108 @@ int BLDCMotor::absoluteZeroAlign() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + // if disabled do nothing + if(!enabled) return; + // shaft angle shaft_angle = shaftAngle(); - electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs)); - - // if current sense linked used true foc architecture - // regulate the current in d axis to be equal to 0 - if(current_sense && controller != ControlType::voltage){ // - // read dq currents - current_measured = current_sense->getFOCCurrents(electrical_angle); - // filter values - current_measured.q = LPF_current_q(current_measured.q); - current_measured.d = LPF_current_d(current_measured.d); - // calculate the phase voltages - voltage.q = PID_current_q(current.q - current_measured.q); - voltage.d = PID_current_d(-current_measured.d); - } + electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs) + zero_electric_angle); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::current: + // read overall current magnitude + current_measured.q = current_sense->getCurrent(electrical_angle); + // filter the value values + current_measured.q = LPF_current_q(current_measured.q); + // calculate the phase voltage + voltage.q = PID_current_q(current.q - current_measured.q); + voltage.d = 0; + break; + case TorqueControlType::foc_current: + // read dq currents + current_measured = current_sense->getFOCCurrents(electrical_angle); + // filter values + current_measured.q = LPF_current_q(current_measured.q); + current_measured.d = LPF_current_d(current_measured.d); + // calculate the phase voltages + voltage.q = PID_current_q(current.q - current_measured.q); + voltage.d = PID_current_d(-current_measured.d); + break; + + default: + // no torque control selected + if(monitor_port) monitor_port->println("MOT: no torque control selected!"); + break; + } + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } // Iterative function running outer loop of the FOC algorithm // Behavior of this function is determined by the motor.controller variable -// It runs either angle, velocity or voltage loop +// It runs either angle, velocity or torque loop // - 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) { + // if disabled do nothing + if(!enabled) return; // set internal target variable if( new_target != NOT_SET ) target = new_target; // get angular velocity shaft_velocity = shaftVelocity(); - // choose control loop + switch (controller) { - case ControlType::torque: - case ControlType::voltage: - if(current_sense) current.q = target; - else voltage.q = target; + case MotionControlType::torque: + if(torque_controller == TorqueControlType::voltage) + voltage.q = target; // if voltage torque control + else + current.q = target; // if current/foc_current torque control break; - case ControlType::angle: + case MotionControlType::angle: // angle set point - // include angle loop shaft_angle_sp = target; + // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - if(current_sense) current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); - else voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + // calculate the torque command + if(torque_controller == TorqueControlType::voltage){ + voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + voltage.d = 0; + }else{ + current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + } break; - case ControlType::velocity: + case MotionControlType::velocity: // velocity set point - // include velocity loop shaft_velocity_sp = target; - if(current_sense) current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); - else voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + // calculate the torque command + if(torque_controller == TorqueControlType::voltage){ + voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + voltage.d = 0; + }else{ + current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + } break; - case ControlType::velocity_openloop: + case MotionControlType::velocity_openloop: // velocity control in open loop // loopFOC should not be called shaft_velocity_sp = target; velocityOpenloop(shaft_velocity_sp); + voltage.d = 0; break; - case ControlType::angle_openloop: + case MotionControlType::angle_openloop: // angle control in open loop // loopFOC should not be called shaft_angle_sp = target; angleOpenloop(shaft_angle_sp); + voltage.d = 0; break; } } + // Method using FOC to set Uq and Ud to the motor at the optimal angle @@ -272,35 +306,35 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { case FOCModulationType::Trapezoid_120 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 static int trap_120_map[6][3] = { - {HIGH_IMPEDANCE,1,-1},{-1,1,HIGH_IMPEDANCE},{-1,HIGH_IMPEDANCE,1},{HIGH_IMPEDANCE,-1,1},{1,-1,HIGH_IMPEDANCE},{1,HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z }; // static int trap_120_state = 0; - sector = 6 * (_normalizeAngle(angle_el + _PI_6 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either // centered == true > driver.volage_limit/2 // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = centered ? (driver->voltage_limit)/2 : Uq; - Ua = trap_120_map[sector][0] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][0] * Uq + center; - Ub = trap_120_map[sector][1] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][1] * Uq + center; - Uc = trap_120_map[sector][2] == HIGH_IMPEDANCE ? HIGH_IMPEDANCE : trap_120_map[sector][2] * Uq + center; + Ua = trap_120_map[sector][0] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][0] * Uq + center; + Ub = trap_120_map[sector][1] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][1] * Uq + center; + Uc = trap_120_map[sector][2] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][2] * Uq + center; break; case FOCModulationType::Trapezoid_150 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 static int trap_150_map[12][3] = { - {HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,HIGH_IMPEDANCE},{-1,1,1},{-1,HIGH_IMPEDANCE,1},{-1,-1,1},{HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,HIGH_IMPEDANCE},{1,-1,-1},{1,HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z }; // static int trap_150_state = 0; - sector = 12 * (_normalizeAngle(angle_el + _PI_6 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either // centered == true > driver.volage_limit/2 // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = centered ? (driver->voltage_limit)/2 : Uq; - Ua = ( trap_150_map[sector][0] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][0] * Uq + center; - Ub = ( trap_150_map[sector][1] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][1] * Uq + center; - Uc = ( trap_150_map[sector][2] == HIGH_IMPEDANCE ) ? HIGH_IMPEDANCE : trap_150_map[sector][2] * Uq + center; + Ua = ( trap_150_map[sector][0] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][0] * Uq + center; + Ub = ( trap_150_map[sector][1] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][1] * Uq + center; + Uc = ( trap_150_map[sector][2] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][2] * Uq + center; break; @@ -310,7 +344,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + zero_electric_angle); + angle_el = _normalizeAngle(angle_el); _ca = _cos(angle_el); _sa = _sin(angle_el); // Inverse park transform @@ -342,7 +376,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // angle normalisation in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + angle_el = _normalizeAngle(angle_el + _PI_2); // find the sector we are in currently sector = floor(angle_el / _PI_3) + 1; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 777f85ae..fd767b40 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,15 +1,15 @@ #include "StepperMotor.h" -// StepperMotor( int phA, int phB, int phC, int pp, int cpr, int en) -// - ph1A, ph1B - motor phase 1 pwm pins -// - ph2A, ph2B - motor phase 2 pwm pins +// StepperMotor(int pp) // - pp - pole pair number -// - enable pin - (optional input) StepperMotor::StepperMotor(int pp) : FOCMotor() { // number od pole pairs pole_pairs = pp; + + // currently supported torque control type + torque_controller = TorqueControlType::voltage; } /** @@ -48,6 +48,8 @@ void StepperMotor::disable() driver->setPwm(0, 0); // disable driver driver->disable(); + // motor status update + enabled = 0; } // enable motor driver void StepperMotor::enable() @@ -56,6 +58,8 @@ void StepperMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0); + // motor status update + enabled = 1; } @@ -86,20 +90,25 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_directi int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: Align sensor."); // align the electrical phases of the motor and sensor - // set angle -90 degrees - + // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); - for (int i = 0; i <=5; i++ ) { - float angle = _3PI_2 + _2PI * i / 6.0; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(200); + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + // move one electrical revolution forward + _delay(500); + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(2); } + // take and angle in the middle float mid_angle = shaftAngle(); - for (int i = 5; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 6.0; + // move one electrical revolution forward + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0 ; setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(200); + _delay(2); } + // determin the direction the sensor moved if (mid_angle < start_angle) { if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); sensor->natural_direction = Direction::CCW; @@ -109,8 +118,8 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); } - // let the motor stabilize for 2 sec - _delay(2000); + // let the motor stabilize for1 sec + _delay(1000); // set sensor to zero sensor->initRelativeZero(); _delay(500); @@ -163,10 +172,13 @@ int StepperMotor::absoluteZeroAlign() { // 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; // shaft angle shaft_angle = shaftAngle(); + electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle, pole_pairs) + zero_electric_angle); // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage.q, voltage.d, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } // Iterative function running outer loop of the FOC algorithm @@ -175,35 +187,40 @@ 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) { + // if disabled do nothing + if(!enabled) return; // set internal target variable if( new_target != NOT_SET ) target = new_target; // get angular velocity shaft_velocity = shaftVelocity(); // choose control loop switch (controller) { - case ControlType::voltage: + case MotionControlType::torque: voltage.q = target; + voltage.d = 0; break; - case ControlType::angle: + case MotionControlType::angle: // angle set point // include angle loop shaft_angle_sp = target; shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + voltage.d = 0; break; - case ControlType::velocity: + case MotionControlType::velocity: // velocity set point // include velocity loop shaft_velocity_sp = target; voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + voltage.d = 0; break; - case ControlType::velocity_openloop: + case MotionControlType::velocity_openloop: // velocity control in open loop // loopFOC should not be called shaft_velocity_sp = target; velocityOpenloop(shaft_velocity_sp); break; - case ControlType::angle_openloop: + case MotionControlType::angle_openloop: // angle control in open loop // loopFOC should not be called shaft_angle_sp = target; @@ -226,7 +243,6 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + zero_electric_angle); float _ca = _cos(angle_el); float _sa = _sin(angle_el); // Inverse park transform diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp new file mode 100644 index 00000000..caa3b572 --- /dev/null +++ b/src/common/base_classes/CurrentSense.cpp @@ -0,0 +1,29 @@ +#include "CurrentSense.h" + +// function used with the foc algorihtm +// calculating DQ currents from phase currents +// - function calculating park and clarke transform of the phase currents +// - using getPhaseCurrents internally +DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ + // read current phase currents + PhaseCurrent_s current = getPhaseCurrents(); + + // calculate clarke transform + float i_alpha, i_beta; + if(!current.c){ + // if only two measured currents + 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 ); + } + + // calculate park transform + float ct = _cos(angle_el); + float st = _sin(angle_el); + DQCurrent_s return_current; + return_current.d = i_alpha * ct + i_beta * st; + return_current.q = i_beta * ct - i_alpha * st; + return return_current; +} \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 6f2c43ab..34e15780 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -11,9 +11,37 @@ class CurrentSense{ public: + /** + * Function intialising the CurrentSense class + * All the necessary intialisations of adc and sync should be implemented here + */ virtual void init(); - virtual PhaseCurrent_s getCurrents(); - virtual DQCurrent_s getFOCCurrents(float angle_el); + /** + * Function rading the phase currents a, b and c + * This function will be used with the foc control throught the function + * CurrentSense::getFOCCurrents(electrical_angle) + * - it returns current c equal to 0 if only two phase measurements available + * + * @return PhaseCurrent_s current values + */ + virtual PhaseCurrent_s getPhaseCurrents(); + /** + * Function reading the magnitude of the current set to the motor + * It returns the abosolute or signed magnitude if possible + * It can receive the motor electrical angle to help with calculation + * This function is used with the current control (not foc) + * + * @param angle_el - electrical angle of the motor (optional) + */ + virtual float getCurrent(float angle_el = 0); + + /** + * Function used for FOC contorl, it reads the DQ currents of the motor + * It uses the function getPhaseCurrents internally + * + * @param angle_el - motor electrical angle + */ + DQCurrent_s getFOCCurrents(float angle_el); }; diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 02ee325f..86855524 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -56,14 +56,14 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) { // shaft angle calculation float FOCMotor::shaftAngle() { - // if no sensor linked return 0 + // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_angle; return sensor->getAngle(); } // shaft velocity calculation float FOCMotor::shaftVelocity() { - // if no sensor linked return 0 - if(!sensor) return 0; + // if no sensor linked return previous value ( for open loop ) + if(!sensor) return shaft_velocity; return LPF_velocity(sensor->getVelocity()); } @@ -80,23 +80,23 @@ void FOCMotor::useMonitoring(Print &print){ void FOCMotor::monitor() { if(!monitor_port) return; switch (controller) { - case ControlType::velocity_openloop: - case ControlType::velocity: + case MotionControlType::velocity_openloop: + case MotionControlType::velocity: monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_velocity_sp); monitor_port->print("\t"); monitor_port->println(shaft_velocity); break; - case ControlType::angle_openloop: - case ControlType::angle: + case MotionControlType::angle_openloop: + case MotionControlType::angle: monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_angle_sp); monitor_port->print("\t"); monitor_port->println(shaft_angle); break; - case ControlType::voltage: + case MotionControlType::torque: monitor_port->print(voltage.q); monitor_port->print("\t"); monitor_port->print(shaft_angle); @@ -193,13 +193,13 @@ int FOCMotor::command(String user_command) { if(GET){ // if get command switch(controller){ - case ControlType::voltage: - if(monitor_port) monitor_port->println("voltage"); + case MotionControlType::torque: + if(monitor_port) monitor_port->println("torque"); break; - case ControlType::velocity: + case MotionControlType::velocity: if(monitor_port) monitor_port->println("velocity"); break; - case ControlType::angle: + case MotionControlType::angle: if(monitor_port) monitor_port->println("angle"); break; default: @@ -208,16 +208,16 @@ int FOCMotor::command(String user_command) { }else{ // if set command switch((int)value){ case 0: - if(monitor_port) monitor_port->println("voltage"); - controller = ControlType::voltage; + if(monitor_port) monitor_port->println("torque"); + controller = MotionControlType::torque; break; case 1: if(monitor_port) monitor_port->println("velocity"); - controller = ControlType::velocity; + controller = MotionControlType::velocity; break; case 2: if(monitor_port) monitor_port->println("angle"); - controller = ControlType::angle; + controller = MotionControlType::angle; break; default: // not valid command errorFlag = 0; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 4ec6532c..457dc31a 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -15,15 +15,23 @@ /** * Motiron control type */ -enum ControlType{ - voltage,//!< Torque control using voltage - torque,//!< Torque control using current +enum MotionControlType{ + torque,//!< Torque control velocity,//!< Velocity motion control angle,//!< Position/angle motion control velocity_openloop, angle_openloop }; +/** + * Motiron control type + */ +enum TorqueControlType{ + voltage, //!< Torque control using voltage + current, //!< Torque control using current + foc_current //!< torque control using dq currents +}; + /** * FOC modulation type */ @@ -122,16 +130,19 @@ class FOCMotor // motor physical parameters float phase_resistance; //!< motor phase resistance int pole_pairs;//!< motor pole pairs number + float zero_electric_angle;//!< absolute zero electric angle - if available // limiting variables float voltage_limit; //!< Voltage limitting variable - global limit float current_limit; //!< Current limitting variable - global limit float velocity_limit; //!< Velocity limitting variable - global limit - float zero_electric_angle;//!> 1 ); + y = * ( float * ) &i; + return number * y; +} diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index d51c97be..3732ea7d 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -7,6 +7,7 @@ #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) #define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) +#define _sqrt(a) (_sqrtAprox(a)) // utility defines #define _2_SQRT3 1.15470053838 @@ -23,22 +24,23 @@ #define _PI_6 0.52359877559 #define NOT_SET -12345.0 -#define HIGH_IMPEDANCE -1234 -#define HIGH_Z HIGH_IMPEDANCE +#define _HIGH_IMPEDANCE -1234 +#define _HIGH_Z _HIGH_IMPEDANCE -// current +// dq current structure struct DQCurrent_s { float d; float q; }; +// phase current structure struct PhaseCurrent_s { float a; float b; float c; }; -// voltage structs +// dq voltage structs struct DQVoltage_s { float d; @@ -75,4 +77,7 @@ float _normalizeAngle(float angle); * @param pole_pairs - number of pole pairs */ float _electricalAngle(float shaft_angle, int pole_pairs); + + +float _sqrtAprox(float value); #endif \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 7f09d9cd..8448437c 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -1,5 +1,10 @@ #include "InlineCurrentSense.h" - +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ pinA = _pinA; pinB = _pinB; @@ -10,33 +15,50 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps } +// Inline sensor init function void InlineCurrentSense::init(){ // calibrate zero offsets calibrateOffsets(); } - +// Function finding zero offsets of the ADC void InlineCurrentSense::calibrateOffsets(){ // find adc offset = zero current voltage offset_ia =0; offset_ib= 0; offset_ic= 0; + // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 500; i++) { offset_ia += _readADCVoltage(pinA); offset_ib += _readADCVoltage(pinB); if(pinC != NOT_SET) offset_ic += _readADCVoltage(pinC); } + // calculate the mean offsets offset_ia = offset_ia / 500.0; offset_ib = offset_ib / 500.0; if(pinC != NOT_SET) offset_ic = offset_ic / 500.0; } -DQCurrent_s InlineCurrentSense::getFOCCurrents(float angle_el){ +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + current.a = gain_adjust_a*(_readADCVoltage(pinA) - offset_ia)*volts_to_amps_ratio;// amps + current.b = gain_adjust_b*(_readADCVoltage(pinB) - offset_ib)*volts_to_amps_ratio;// amps + current.c = (pinC == NOT_SET) ? 0 : gain_adjust_c*(_readADCVoltage(pinC) - offset_ic)*volts_to_amps_ratio; // amps + return current; +} + +// get current magnitude +// - absolute - if no electrical_angle provided +// - signed - if angle provided +float InlineCurrentSense::getCurrent(float motor_electrical_angle){ // read current phase currents - PhaseCurrent_s current = getCurrents(); + PhaseCurrent_s current = getPhaseCurrents(); + // currnet sign - if motor angle not provided the magnitude is always positive + float sign = 1; // calculate clarke transform float i_alpha, i_beta; - if(pinC == NOT_SET){ + if(!current.c){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; @@ -45,19 +67,11 @@ DQCurrent_s InlineCurrentSense::getFOCCurrents(float angle_el){ i_beta = _2_SQRT3 *( current.b - current.c ); } - // calculate park transform - float ct = _cos(angle_el); - float st = _sin(angle_el); - DQCurrent_s return_current; - return_current.d = i_alpha * ct + i_beta * st; - return_current.q = i_beta * ct - i_alpha * st; - return return_current; -} - -PhaseCurrent_s InlineCurrentSense::getCurrents(){ - PhaseCurrent_s current; - current.a = (_readADCVoltage(pinA) - offset_ia)*volts_to_amps_ratio;// amps - current.b = -(_readADCVoltage(pinB) - offset_ib)*volts_to_amps_ratio;// amps - current.c = (pinC == NOT_SET) ? -(current.a + current.b) : (_readADCVoltage(pinC) - offset_ic)*volts_to_amps_ratio; // amps - return current; + // if motor angle provided function returns signed value of the current + // determine the sign of the current + // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 + if(motor_electrical_angle) + sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; + // return current magnitude + return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); } diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 17565073..3c9323c6 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -12,31 +12,42 @@ class InlineCurrentSense: public CurrentSense{ public: /** InlineCurrentSense class constructor - @param phA A phase pwm pin - @param phB B phase pwm pin - @param phC C phase pwm pin (optional) + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) */ InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + // CurrentSense interface implementing functions void init() override; + PhaseCurrent_s getPhaseCurrents() override; + float getCurrent( float angle_el = 0) override; - PhaseCurrent_s getCurrents() override; - DQCurrent_s getFOCCurrents(float angle_el) override; + // ADC measuremnet gain adjustment for each phase + // if (shunt+amp combination) measures inverse current you can set it to -1 for example + // this should be automated later + int gain_adjust_a = 1; //!< phase A gain adjust + int gain_adjust_b = 1; //!< phase B gain adjust + int gain_adjust_c = 1; //!< phase C gain adjust + + private: // hardware variables int pinA; //!< pin A analog pin for current measurement int pinB; //!< pin B analog pin for current measurement int pinC; //!< pin C analog pin for current measurement - private: - - + // gain variables double shunt_resistor; //!< Shunt resistor value double amp_gain; //!< amp gain value double volts_to_amps_ratio; //!< Volts to amps ratio - // function finding the zero offsets + /** + * Function finding zero offsets of the ADC + */ void calibrateOffsets(); double offset_ia; //!< zero current A voltage value (center of the adc reading) double offset_ib; //!< zero current B voltage value (center of the adc reading) diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 8ff3a0dd..40926165 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -4,7 +4,10 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" -/** +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltage(const int pinA); diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 6b74ad3c..708e750a 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -23,6 +23,7 @@ voltage = raw_adc * 5.0/1024.0; #endif +// function reading an ADC value and returning the read voltage float _readADCVoltage(const int pinA){ int raw_adc = analogRead(pinA); return raw_adc * (float)_ADC_VOLTAGE / (float)_ADC_RESOLUTION; diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index c795fce1..078dd0c8 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -66,15 +66,17 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // disable if needed - if(enableA_pin != NOT_SET ) digitalWrite(enableA_pin, Ua == HIGH_IMPEDANCE ? LOW : HIGH); - if(enableB_pin != NOT_SET ) digitalWrite(enableB_pin, Ub == HIGH_IMPEDANCE ? LOW : HIGH); - if(enableC_pin != NOT_SET ) digitalWrite(enableC_pin, Uc == HIGH_IMPEDANCE ? LOW : HIGH); + if(enableA_pin != NOT_SET && enableB_pin != NOT_SET && enableC_pin != NOT_SET ){ + digitalWrite(enableA_pin, Ua == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableB_pin, Ub == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableC_pin, Uc == _HIGH_IMPEDANCE ? LOW : HIGH); + } // voltage of the high-impedance phase if it is not possible to disable it // will be in exactly in the middle of the other two - if (Ua == HIGH_IMPEDANCE) Ua = (Ub + Uc)/2; - if (Ub == HIGH_IMPEDANCE) Ub = (Ua + Uc)/2; - if (Uc == HIGH_IMPEDANCE) Uc = (Ua + Ub)/2; + if (Ua == _HIGH_IMPEDANCE) Ua = (Ub + Uc)/2; + if (Ub == _HIGH_IMPEDANCE) Ub = (Ua + Uc)/2; + if (Uc == _HIGH_IMPEDANCE) Uc = (Ua + Ub)/2; // limit the voltage in driver Ua = _constrain(Ua, 0.0, voltage_limit); From 285ccec46193568ecb300629240c79419ad8860e Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 3 Feb 2021 17:26:19 +0100 Subject: [PATCH 072/749] added info --- README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index b5cb13b9..a82ee854 100644 --- a/README.md +++ b/README.md @@ -13,11 +13,18 @@ Therefore this is an attempt to: - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop a modular low-power BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). -##### NEXT RELEASE 📢: SimpleFOClibrary v2.0.3 +##### NEXT RELEASE 📢: SimpleFOClibrary v2.1 > #### Implemented features in dev branch > - 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 ) +> - **Initial current sensing support** +> - Inline current sensors +> - **Implemented real torque control** +> - using voltage +> - using current magnitude (one current) +> - using FOC currents ( d-q currents ) - real foc control ## Arduino *SimpleFOCShield* v2.0.2 From 58f7b7d9eb46ff05d0c56d7a4055e3b9e4cc4c29 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Feb 2021 08:40:38 +0100 Subject: [PATCH 073/749] updated example codes --- README.md | 2 +- .../bluepill_position_control.ino | 2 +- .../bluepill_position_control.ino | 2 +- .../full_control_serial.ino | 2 +- .../full_control_serial.ino | 2 +- .../esp32_position_control.ino | 2 +- .../esp32_position_control.ino | 2 +- .../position_control/position_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../open_loop_position_example.ino | 2 +- .../open_loop_velocity_example.ino | 2 +- .../encoder/angle_control/angle_control.ino | 2 +- .../angle_control/angle_control.ino | 2 +- .../angle_control/angle_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../true_foc_control/true_foc_control.ino | 126 ------------------ .../velocity_control/velocity_control.ino | 2 +- .../hall_sensor/velocity_control.ino | 2 +- .../velocity_control/velocity_control.ino | 2 +- .../full_control_serial.ino | 2 +- .../full_control_serial.ino | 2 +- .../full_control_serial.ino | 2 +- .../simplefoc_osc_esp32_3pwm.ino | 2 +- .../alignment_and_cogging_test.ino | 2 +- .../find_pole_pairs_number.ino | 4 +- .../find_pole_pairs_number.ino | 4 +- .../find_sensor_offset_and_direction.ino | 2 +- src/BLDCMotor.cpp | 2 + src/SimpleFOC.h | 2 +- src/StepperMotor.cpp | 3 +- 32 files changed, 35 insertions(+), 158 deletions(-) rename examples/motion_control/{torque_voltage_control => torque_control}/encoder/voltage_control/voltage_control.ino (98%) rename examples/motion_control/{torque_voltage_control => torque_control}/hall_sensor/voltage_control/voltage_control.ino (98%) rename examples/motion_control/{torque_voltage_control => torque_control}/magnetic_sensor/voltage_control/voltage_control.ino (98%) delete mode 100644 examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino diff --git a/README.md b/README.md index a82ee854..a52f5ca9 100644 --- a/README.md +++ b/README.md @@ -155,7 +155,7 @@ void setup() { motor.linkDriver(&driver); // set control loop type to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // initialize motor motor.init(); diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 3a850745..5a1c0762 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -44,7 +44,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // contoller configuration // default parameters in defaults.h 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 b80b614a..aa969e29 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 @@ -44,7 +44,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index ed0404d7..99159eb3 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -69,7 +69,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // contoller configuration based on the controll type motor.PID_velocity.P = 0.2; diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 3de0f4d7..bfdbdcdf 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -70,7 +70,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // contoller configuration based on the controll type motor.PID_velocity.P = 0.2; diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 0fe695d1..6341fa6a 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -38,7 +38,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // contoller configuration // default parameters in defaults.h diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 617f04fc..61873344 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -42,7 +42,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 5235c624..6498e5f9 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -56,7 +56,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index 9e847da4..097d1cb4 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -60,7 +60,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // use monitoring with serial for motor init // comment out if not needed diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 10d0c5c1..edf239bf 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -25,7 +25,7 @@ void setup() { motor.voltage_limit = 3; // [V] motor.velocity_limit = 20; // [rad/s] // open loop control config - motor.controller = ControlType::angle_openloop; + motor.controller = MotionControlType::angle_openloop; // init motor hardware motor.init(); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index e5f99817..1aa0a93d 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -26,7 +26,7 @@ void setup() { motor.velocity_limit = 20; // [rad/s] // open loop control config - motor.controller = ControlType::velocity_openloop; + motor.controller = MotionControlType::velocity_openloop; // init motor hardware motor.init(); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index b237b693..3b50a21b 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -70,7 +70,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index d5be41b5..f669887c 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -64,7 +64,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 887e8e3c..9fbbdcc9 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -41,7 +41,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::angle; + motor.controller = MotionControlType::angle; // contoller configuration // default parameters in defaults.h diff --git a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino similarity index 98% rename from examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino rename to examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 8d904621..86451759 100644 --- a/examples/motion_control/torque_voltage_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -47,7 +47,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // use monitoring with serial Serial.begin(115200); diff --git a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino similarity index 98% rename from examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino rename to examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 0e942382..90fe656a 100644 --- a/examples/motion_control/torque_voltage_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -48,7 +48,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // use monitoring with serial Serial.begin(115200); diff --git a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino similarity index 98% rename from examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino rename to examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index 25ee104b..13414707 100644 --- a/examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -39,7 +39,7 @@ void setup() { // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // use monitoring with serial Serial.begin(115200); diff --git a/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino b/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino deleted file mode 100644 index 0ed17c89..00000000 --- a/examples/motion_control/true_foc_control/encoder/true_foc_control/true_foc_control.ino +++ /dev/null @@ -1,126 +0,0 @@ -/** - - Torque control example using current control loop. - - This example is based on the Arduino SimpleFOCShield V2.0 - The current is measured inline with the shunt resistor or R= 0.01 Ohm, and amplification gain og G = 50 -*/ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 11, 8); - -// encoder instance -Encoder encoder = Encoder(2, 3, 500); -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -// current sensor -// InlineCurrentSense(shunt_resistor, gain, pinA, pinB, (optional pinC)) -InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); - -void setup() { - - // initialize encoder sensor hardware - encoder.init(); - encoder.enableInterrupts(doA, doB); - // link the motor to the sensor - motor.linkSensor(&encoder); - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.pwm_frequency = 50000; - driver.init(); - // link driver - motor.linkDriver(&driver); - - // set motion control loop to be used - // ControlType::torque, ControlType::velocity or ControlType::angle - motor.controller = ControlType::velocity; - - // foc currnet control parameters (all default) - motor.PID_current_q.P = 5; - motor.PID_current_q.I= 1000; - motor.PID_current_d.P= 5; - motor.PID_current_d.I = 1000; - motor.LPF_current_q.Tf = 0.002; // 1ms default - motor.LPF_current_d.Tf = 0.002; // 1ms default - - // motion control parameters - motor.PID_velocity.P = 0.05; - motor.PID_velocity.I = 1; - motor.PID_velocity.D = 0; - motor.LPF_velocity.Tf = 0.005; - motor.current_limit = 0.5; // 0.5 Amps limit - - _delay(1000); - // initialise the current sensing - current_sense.init(); - // linking the current sensing - motor.linkCurrentSense(¤t_sense); - // initialize motor - motor.init(); - // align sensor and start FOC - motor.initFOC(); - - // use monitoring with serial - Serial.begin(250000); - - Serial.println("Motor ready."); -} - - -// target voltage to be set to the motor -float target_angle = 0; - -// counting variable for user display -long t = 0; - - void loop() { - - // set the phase voltages Ud and Uq - motor.loopFOC(); - motor.move(target_angle); - - // show user the currents - if (!t) { - Serial.print(motor.current_measured.q*1000); // mAmps - Serial.print("\t"); - Serial.println(motor.current_measured.d*1000); // mAmps - } - t > 20 ? t = 0 : t++; - - // communicate with the user - serialReceiveUserCommand(); -} - - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle [radian]: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } -} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index 50bde4b7..3e1b35be 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -71,7 +71,7 @@ void setup() { motor.velocity_index_search = 3; // set motion control loop to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // contoller configuration // default parameters in defaults.h diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index cb9b9a3d..fc20d1bf 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -61,7 +61,7 @@ void setup() { motor.voltage_sensor_align = 3; // set motion control loop to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // contoller configuration // default parameters in defaults.h diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 1fbfb8ff..83ef0a51 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -40,7 +40,7 @@ void setup() { motor.linkDriver(&driver); // set motion control loop to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // contoller configuration // default parameters in defaults.h diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 5ed5ea16..8f6ff1c0 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -72,7 +72,7 @@ void setup() { // choose FOC modulation motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // contoller configuration based on the controll type motor.PID_velocity.P = 0.2; diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index ddbcfe91..945197b5 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -80,7 +80,7 @@ void setup() { // choose FOC modulation motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // contoller configuration based on the controll type motor.PID_velocity.P = 0.2; motor.PID_velocity.I = 20; diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index bb7786cc..308c66c9 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -71,7 +71,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // contoller configuration based on the control type motor.PID_velocity.P = 0.2; diff --git a/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino b/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino index f296dc01..de13e7ae 100644 --- a/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino @@ -90,7 +90,7 @@ void setup() { driver.voltage_power_supply = 9; driver.init(); motor.linkDriver(&driver); - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; motor.PID_velocity.P = 0.2; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0.001; diff --git a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino index a8bd27a7..f283b3b1 100644 --- a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -88,7 +88,7 @@ void setup() { motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor.controller = ControlType::angle_openloop; + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=motor.voltage_sensor_align; sensor.init(); diff --git a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index b3c26556..46d18640 100644 --- a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -59,7 +59,7 @@ void setup() { float pp_search_angle = 6*M_PI; // search electrical angle to turn // move motor to the electrical angle 0 - motor.controller = ControlType::angle_openloop; + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); @@ -110,7 +110,7 @@ void setup() { // set FOC loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // set the pole pair number to the motor motor.pole_pairs = pp; //align encoder and start FOC diff --git a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index 5fcf27bd..26f9c528 100644 --- a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -64,7 +64,7 @@ void setup() { float pp_search_angle = 6*M_PI; // search electrical angle to turn // move motor to the electrical angle 0 - motor.controller = ControlType::angle_openloop; + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); @@ -116,7 +116,7 @@ void setup() { // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // set the pole pair number to the motor motor.pole_pairs = pp; //align sensor and start FOC diff --git a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 54eb1d17..fe8ea8f2 100644 --- a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -42,7 +42,7 @@ void setup() { motor.voltage_sensor_align = 7; // set motion control loop to be used - motor.controller = ControlType::voltage; + motor.controller = MotionControlType::torque; // initialize motor motor.init(); diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 574ef63b..2ed49c58 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -10,6 +10,8 @@ BLDCMotor::BLDCMotor(int pp, float _R) pole_pairs = pp; // save phase resistance number phase_resistance = _R; + // torque control type is voltage by default + torque_controller = TorqueControlType::voltage; } diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 82d3e375..27b9ab84 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -69,7 +69,7 @@ void setup() { motor.linkDriver(&driver); // set control loop type to be used - motor.controller = ControlType::velocity; + motor.controller = MotionControlType::velocity; // initialize motor motor.init(); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index fd767b40..a631e288 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -8,7 +8,8 @@ StepperMotor::StepperMotor(int pp) // number od pole pairs pole_pairs = pp; - // currently supported torque control type + // torque control type is voltage by default + // current and foc_current not supported yet torque_controller = TorqueControlType::voltage; } From 06a74847cf10372f009a18bb571f8ce9b4f715dd Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Feb 2021 20:59:01 +0100 Subject: [PATCH 074/749] updates strings with F macro --- README.md | 5 ++++ .../bluepill_position_control.ino | 4 +-- .../bluepill_position_control.ino | 4 +-- .../full_control_serial.ino | 8 +++--- .../full_control_serial.ino | 8 +++--- .../esp32_position_control.ino | 4 +-- .../esp32_position_control.ino | 4 +-- .../position_control/position_control.ino | 4 +-- .../voltage_control/voltage_control.ino | 4 +-- .../encoder/angle_control/angle_control.ino | 4 +-- .../angle_control/angle_control.ino | 4 +-- .../angle_control/angle_control.ino | 4 +-- .../voltage_control/voltage_control.ino | 4 +-- .../voltage_control/voltage_control.ino | 4 +-- .../voltage_control/voltage_control.ino | 4 +-- .../velocity_control/velocity_control.ino | 4 +-- .../hall_sensor/velocity_control.ino | 4 +-- .../velocity_control/velocity_control.ino | 4 +-- .../full_control_serial.ino | 2 +- .../full_control_serial.ino | 2 +- .../full_control_serial.ino | 2 +- .../alignment_and_cogging_test.ino | 16 ++++++------ .../find_pole_pairs_number.ino | 20 +++++++------- .../find_pole_pairs_number.ino | 24 ++++++++--------- src/BLDCMotor.cpp | 26 +++++++++---------- src/StepperMotor.cpp | 24 ++++++++--------- src/common/foc_utils.cpp | 5 ++-- src/common/foc_utils.h | 11 +++++--- 28 files changed, 112 insertions(+), 101 deletions(-) diff --git a/README.md b/README.md index a52f5ca9..c540599a 100644 --- a/README.md +++ b/README.md @@ -19,12 +19,17 @@ Therefore this is an attempt to: > - Support for Arduino DUE - everything except the 6PWM mode > - Support for ATMega328pb > - bugfix for the Teensy boards (setting 3pwm ) +> - included F macro for shrinking string memory usage - moved to programming memory > - **Initial current sensing support** > - Inline current sensors > - **Implemented real torque control** > - using voltage > - using current magnitude (one current) > - using FOC currents ( d-q currents ) - real foc control +> +> BEWARE 📢 slight API changes included +> - `ControlType` renamed into `MotionControlType` +> - `ControlType::voltage` does not exist any more now - `MotionControlType::torque` ## Arduino *SimpleFOCShield* v2.0.2 diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 5a1c0762..b1e6fcef 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -78,8 +78,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F(("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index aa969e29..ee6a3a8d 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 @@ -76,8 +76,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 99159eb3..4f6ede28 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -100,10 +100,10 @@ void setup() { motor.target = 2; - Serial.println("Full control example: "); - Serial.println("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "); - Serial.println("Initial motion control loop is voltage loop."); - Serial.println("Initial target voltage 2V."); + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index bfdbdcdf..627c2c43 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -101,10 +101,10 @@ void setup() { motor.target = 2; - Serial.println("Full control example: "); - Serial.println("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "); - Serial.println("Initial motion control loop is voltage loop."); - Serial.println("Initial target voltage 2V."); + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); _delay(1000); } diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 6341fa6a..7978567f 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -72,8 +72,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 61873344..39f322b6 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -74,8 +74,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 6498e5f9..3038b3ab 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -90,8 +90,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index 097d1cb4..fbb27a55 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -76,8 +76,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 3b50a21b..5a352b6e 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -105,8 +105,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index f669887c..9c6c4192 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -99,8 +99,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 9fbbdcc9..5d346243 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -74,8 +74,8 @@ void setup() { motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 86451759..40f90142 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -59,8 +59,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 90fe656a..cf64070f 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -60,8 +60,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index 13414707..13273b92 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -51,8 +51,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index 3e1b35be..27d97576 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -99,8 +99,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target velocity using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index fc20d1bf..0f3441e7 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -89,8 +89,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target velocity using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 83ef0a51..f47bd078 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -70,8 +70,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); - Serial.println("Motor ready."); - Serial.println("Set the target velocity using serial terminal:"); + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 8f6ff1c0..bcfb846a 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -105,7 +105,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); _delay(1000); } diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 945197b5..3cec7de6 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -112,7 +112,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); _delay(1000); } diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 308c66c9..410ec48b 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -104,7 +104,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); _delay(1000); } diff --git a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino index f283b3b1..8292ecf4 100644 --- a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -59,18 +59,18 @@ void testAlignmentAndCogging(int direction) { } Serial.println(); - Serial.println("ALIGNMENT AND COGGING REPORT"); + Serial.println(F("ALIGNMENT AND COGGING REPORT")); Serial.println(); - Serial.print("Direction: "); + Serial.print(F("Direction: ")); Serial.println(direction); - Serial.print("Mean error (alignment): "); + Serial.print(F("Mean error (alignment): ")); Serial.print(mean); Serial.println(" deg (electrical)"); - Serial.print("Standard Deviation (cogging): "); + Serial.print(F("Standard Deviation (cogging): ")); Serial.print(sqrt(stDevSum/sample_count)); - Serial.println(" deg (electrical)"); + Serial.println(F(" deg (electrical)")); Serial.println(); - Serial.println("Plotting 3rd column of data (electricAngleError) will likely show sinusoidal cogging pattern with a frequency of 4xpole_pairs per rotation"); + Serial.println(F("Plotting 3rd column of data (electricAngleError) will likely show sinusoidal cogging pattern with a frequency of 4xpole_pairs per rotation")); Serial.println(); } @@ -101,12 +101,12 @@ void setup() { testAlignmentAndCogging(1); motor.move(0); - Serial.println("Press any key to test in CCW direction"); + Serial.println(F("Press any key to test in CCW direction")); while (!Serial.available()) { } testAlignmentAndCogging(-1); - Serial.println("Complete"); + Serial.println(F("Complete")); motor.voltage_limit = 0; motor.move(0); diff --git a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index 46d18640..e6a1d20b 100644 --- a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -84,9 +84,9 @@ void setup() { // calculate the pole pair number int pp = round((pp_search_angle)/(angle_end-angle_begin)); - Serial.print("Estimated PP : "); + Serial.print(F("Estimated PP : ")); Serial.println(pp); - Serial.println("PP = Electrical angle / Encoder angle "); + Serial.println(F("PP = Electrical angle / Encoder angle ")); Serial.print(pp_search_angle*180/M_PI); Serial.print("/"); Serial.print((angle_end-angle_begin)*180/M_PI); @@ -97,15 +97,15 @@ void setup() { // a bit of monitoring the result if(pp <= 0 ){ - Serial.println("PP number cannot be negative"); - Serial.println(" - Try changing the search_voltage value or motor/encoder configuration."); + Serial.println(F("PP number cannot be negative")); + Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration.")); return; }else if(pp > 30){ - Serial.println("PP number very high, possible error."); + Serial.println(F("PP number very high, possible error.")); }else{ - Serial.println("If PP is estimated well your motor should turn now!"); - Serial.println(" - If it is not moving try to relaunch the program!"); - Serial.println(" - You can also try to adjust the target voltage using serial terminal!"); + Serial.println(F("If PP is estimated well your motor should turn now!")); + Serial.println(F(" - If it is not moving try to relaunch the program!")); + Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } @@ -117,8 +117,8 @@ void setup() { motor.initFOC(); _delay(1000); - Serial.println("\n Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("\n Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); } // uq voltage diff --git a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index 26f9c528..c0711b64 100644 --- a/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -90,28 +90,28 @@ void setup() { // calculate the pole pair number int pp = round((pp_search_angle)/(angle_end-angle_begin)); - Serial.print("Estimated PP : "); + Serial.print(F("Estimated PP : ")); Serial.println(pp); - Serial.println("PP = Electrical angle / Encoder angle "); + Serial.println(F("PP = Electrical angle / Encoder angle ")); Serial.print(pp_search_angle*180/M_PI); - Serial.print("/"); + Serial.print(F("/")); Serial.print((angle_end-angle_begin)*180/M_PI); - Serial.print(" = "); + Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); // a bit of monitoring the result if(pp <= 0 ){ - Serial.println("PP number cannot be negative"); - Serial.println(" - Try changing the search_voltage value or motor/sensor configuration."); + Serial.println(F("PP number cannot be negative")); + Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration.")); return; }else if(pp > 30){ - Serial.println("PP number very high, possible error."); + Serial.println(F("PP number very high, possible error.")); }else{ - Serial.println("If PP is estimated well your motor should turn now!"); - Serial.println(" - If it is not moving try to relaunch the program!"); - Serial.println(" - You can also try to adjust the target voltage using serial terminal!"); + Serial.println(F("If PP is estimated well your motor should turn now!")); + Serial.println(F(" - If it is not moving try to relaunch the program!")); + Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } @@ -123,8 +123,8 @@ void setup() { motor.initFOC(); _delay(1000); - Serial.println("\n Motor ready."); - Serial.println("Set the target voltage using serial terminal:"); + Serial.println(F("\n Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); } // uq voltage diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 2ed49c58..8ce63f8b 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -24,7 +24,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { - if(monitor_port) monitor_port->println("MOT: Initialise variables."); + if(monitor_port) monitor_port->println(F("MOT: Initialise variables.")); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && phase_resistance != NOT_SET ) { @@ -51,7 +51,7 @@ void BLDCMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println("MOT: Enable driver."); + if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); } @@ -97,13 +97,13 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction exit_flag = alignSensor(); _delay(500); } - if(monitor_port) monitor_port->println("MOT: Motor ready."); + if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); return exit_flag; } // Encoder alignment to electrical 0 angle int BLDCMotor::alignSensor() { - if(monitor_port) monitor_port->println("MOT: Align sensor."); + if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); @@ -125,12 +125,12 @@ int BLDCMotor::alignSensor() { } // determin the direction the sensor moved if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); sensor->natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { - if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); } else{ - if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); } // let the motor stabilize for1 sec @@ -145,9 +145,9 @@ int BLDCMotor::alignSensor() { int exit_flag = absoluteZeroAlign(); _delay(500); if(monitor_port){ - if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); - if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); - else monitor_port->println("MOT: Not available!"); + if(exit_flag< 0 ) monitor_port->println(F("MOT: Error: Not found!")); + if(exit_flag> 0 ) monitor_port->println(F("MOT: Success!")); + else monitor_port->println(F("MOT: Not available!")); } return exit_flag; } @@ -157,12 +157,12 @@ int BLDCMotor::alignSensor() { // - to the index int BLDCMotor::absoluteZeroAlign() { - if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + if(monitor_port) monitor_port->println(F("MOT: Absolute zero align.")); // if no absolute zero return if(!sensor->hasAbsoluteZero()) return 0; - if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println(F("MOT: Searching...")); // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ loopFOC(); @@ -219,7 +219,7 @@ void BLDCMotor::loopFOC() { default: // no torque control selected - if(monitor_port) monitor_port->println("MOT: no torque control selected!"); + if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); break; } diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index a631e288..2705692b 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -22,7 +22,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { // init hardware pins void StepperMotor::init() { - if(monitor_port) monitor_port->println("MOT: Init variables."); + if(monitor_port) monitor_port->println(F("MOT: Init variables.")); // sanity check for the voltage limit configuration if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; @@ -35,7 +35,7 @@ void StepperMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println("MOT: Enable."); + if(monitor_port) monitor_port->println(F("MOT: Enable.")); enable(); _delay(500); @@ -83,13 +83,13 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_directi exit_flag = alignSensor(); _delay(500); } - if(monitor_port) monitor_port->println("MOT: Motor ready."); + if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); return exit_flag; } // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { - if(monitor_port) monitor_port->println("MOT: Align sensor."); + if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); @@ -111,12 +111,12 @@ int StepperMotor::alignSensor() { } // determin the direction the sensor moved if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); sensor->natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { - if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); } else{ - if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); } // let the motor stabilize for1 sec @@ -131,9 +131,9 @@ int StepperMotor::alignSensor() { int exit_flag = absoluteZeroAlign(); _delay(500); if(monitor_port){ - if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); - if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); - else monitor_port->println("MOT: Not available!"); + if(exit_flag< 0 ) monitor_port->println(F("MOT: Error: Not found!")); + if(exit_flag> 0 ) monitor_port->println(F("MOT: Success!")); + else monitor_port->println(F("MOT: Not available!")); } return exit_flag; } @@ -143,12 +143,12 @@ int StepperMotor::alignSensor() { // - to the index int StepperMotor::absoluteZeroAlign() { - if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + if(monitor_port) monitor_port->println(F("MOT: Absolute zero align.")); // if no absolute zero return if(!sensor->hasAbsoluteZero()) return 0; - if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println(F("MOT: Searching...")); // search the absolute zero with small velocity while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ loopFOC(); diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fbdd8130..9ea53a65 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,9 +1,10 @@ #include "foc_utils.h" // int array instead of float array +// 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 -int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; +const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; // function approximating the sine calculation by using fixed size array // ~40us (float array) @@ -56,7 +57,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root -float _sqrtAprox(float number) {//low in fat +float _sqrtApprox(float number) {//low in fat unsigned long i; float x, y; y = number; diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 3732ea7d..610ac7db 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -7,7 +7,7 @@ #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) #define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) -#define _sqrt(a) (_sqrtAprox(a)) +#define _sqrt(a) (_sqrtApprox(a)) // utility defines #define _2_SQRT3 1.15470053838 @@ -78,6 +78,11 @@ float _normalizeAngle(float angle); */ float _electricalAngle(float shaft_angle, int pole_pairs); - -float _sqrtAprox(float value); +/** + * Function approximating square root function + * - using fast inverse square root + * + * @param value - number + */ +float _sqrtApprox(float value); #endif \ No newline at end of file From 15d70f2f04e22f1c4e6c4f1c03eb3bd1a9d2e77e Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 7 Feb 2021 09:24:36 +0100 Subject: [PATCH 075/749] svpwm full imp + disable phase support for 3pwm + sqrt approx + a lot of optim --- README.md | 9 +- src/BLDCMotor.cpp | 92 +++++++++++++------ src/common/base_classes/BLDCDriver.h | 9 ++ src/common/base_classes/FOCMotor.cpp | 66 +++++++++++-- src/common/base_classes/FOCMotor.h | 4 +- src/common/defaults.h | 6 +- src/common/foc_utils.cpp | 8 +- src/common/foc_utils.h | 3 +- src/current_sense/InlineCurrentSense.cpp | 2 + src/current_sense/hardware_api.h | 9 ++ .../hardware_specific/generic_mcu.cpp | 40 ++++++-- src/drivers/BLDCDriver3PWM.cpp | 19 ++-- src/drivers/BLDCDriver3PWM.h | 8 ++ src/drivers/BLDCDriver6PWM.cpp | 6 ++ src/drivers/BLDCDriver6PWM.h | 9 ++ 15 files changed, 226 insertions(+), 64 deletions(-) diff --git a/README.md b/README.md index c540599a..c9a5dc5b 100644 --- a/README.md +++ b/README.md @@ -26,10 +26,13 @@ Therefore this is an attempt to: > - using voltage > - using current magnitude (one current) > - using FOC currents ( d-q currents ) - real foc control -> +> - disable phase support for 3pwm driver +> - not yet for 6pwm +> - SVPWM full implementation d+q axis +> > BEWARE 📢 slight API changes included -> - `ControlType` renamed into `MotionControlType` -> - `ControlType::voltage` does not exist any more now - `MotionControlType::torque` +> - `MotionControlType` renamed into `MotionControlType` +> - `MotionControlType::voltage` does not exist any more now - `MotionControlType::torque` ## Arduino *SimpleFOCShield* v2.0.2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8ce63f8b..6a781cb7 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -199,22 +199,22 @@ void BLDCMotor::loopFOC() { break; case TorqueControlType::current: // read overall current magnitude - current_measured.q = current_sense->getCurrent(electrical_angle); + current.q = current_sense->getCurrent(electrical_angle); // filter the value values - current_measured.q = LPF_current_q(current_measured.q); + current.q = LPF_current_q(current.q); // calculate the phase voltage - voltage.q = PID_current_q(current.q - current_measured.q); + voltage.q = PID_current_q(current_sp - current.q); voltage.d = 0; break; case TorqueControlType::foc_current: // read dq currents - current_measured = current_sense->getFOCCurrents(electrical_angle); + current = current_sense->getFOCCurrents(electrical_angle); // filter values - current_measured.q = LPF_current_q(current_measured.q); - current_measured.d = LPF_current_d(current_measured.d); + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); // calculate the phase voltages - voltage.q = PID_current_q(current.q - current_measured.q); - voltage.d = PID_current_d(-current_measured.d); + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); break; default: @@ -245,7 +245,7 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage) voltage.q = target; // if voltage torque control else - current.q = target; // if current/foc_current torque control + current_sp = target; // if current/foc_current torque control break; case MotionControlType::angle: // angle set point @@ -257,7 +257,7 @@ void BLDCMotor::move(float new_target) { voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control voltage.d = 0; }else{ - current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control } break; case MotionControlType::velocity: @@ -268,7 +268,7 @@ void BLDCMotor::move(float new_target) { voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control voltage.d = 0; }else{ - current.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control } break; case MotionControlType::velocity_openloop: @@ -317,9 +317,23 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = centered ? (driver->voltage_limit)/2 : Uq; - Ua = trap_120_map[sector][0] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][0] * Uq + center; - Ub = trap_120_map[sector][1] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][1] * Uq + center; - Uc = trap_120_map[sector][2] == _HIGH_IMPEDANCE ? _HIGH_IMPEDANCE : trap_120_map[sector][2] * Uq + center; + if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){ + Ua= center; + Ub = trap_120_map[sector][1] * Uq + center; + Uc = trap_120_map[sector][2] * Uq + center; + driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible + }else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){ + Ua = trap_120_map[sector][0] * Uq + center; + Ub = center; + Uc = trap_120_map[sector][2] * Uq + center; + driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible + }else{ + Ua = trap_120_map[sector][0] * Uq + center; + Ub = trap_120_map[sector][1] * Uq + center; + Uc = center; + driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible + } + break; case FOCModulationType::Trapezoid_150 : @@ -334,9 +348,22 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = centered ? (driver->voltage_limit)/2 : Uq; - Ua = ( trap_150_map[sector][0] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][0] * Uq + center; - Ub = ( trap_150_map[sector][1] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][1] * Uq + center; - Uc = ( trap_150_map[sector][2] == _HIGH_IMPEDANCE ) ? _HIGH_IMPEDANCE : trap_150_map[sector][2] * Uq + center; + if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){ + Ua= center; + Ub = trap_150_map[sector][1] * Uq + center; + Uc = trap_150_map[sector][2] * Uq + center; + driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible + }else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){ + Ua = trap_150_map[sector][0] * Uq + center; + Ub = center; + Uc = trap_150_map[sector][2] * Uq + center; + driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible + }else{ + Ua = trap_150_map[sector][0] * Uq + center; + Ub = trap_150_map[sector][1] * Uq + center; + Uc = center; + driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible + } break; @@ -353,10 +380,12 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + // center = centered ? (driver->voltage_limit)/2 : Uq; + center = driver->voltage_limit/2; // Clarke transform - Ua = Ualpha + driver->voltage_limit/2; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Ua = Ualpha + center; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + center; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + center; if (!centered) { float Umin = min(Ua, min(Ub, Uc)); @@ -371,20 +400,27 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm // https://www.youtube.com/watch?v=QMSWUMEAejg - // if negative voltages change inverse the phase - // angle + 180degrees - if(Uq < 0) angle_el += _PI; - Uq = abs(Uq); - + // the algorithm goes + // 1) Ualpha, Ubeta + // 2) Uout = sqrt(Ualpha^2 + Ubeta^2) + // 3) angle_el = atan2(Ubeta, Ualpha) + // + // equivalent to 2) because the magnitude does not change is: + // Uout = sqrt(Ud^2 + Uq^2) + // equivalent to 3) is + // angle_el = angle_el + atan2(Uq,Ud) + + // _sqrt is an approx of sqrt (3-4% error) + float Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; // angle normalisation in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + _PI_2); + angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); // find the sector we are in currently sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (centered) { diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 44ed5786..eeec2e8e 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -28,6 +28,15 @@ class BLDCDriver{ * @param Uc - phase C voltage */ virtual void setPwm(float Ua, float Ub, float Uc); + + /** + * Set phase voltages to the harware + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + * @param sa - phase C state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(int sa, int sb, int sc); }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 86855524..2474a173 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -28,8 +28,9 @@ FOCMotor::FOCMotor() voltage.d = 0; voltage.q = 0; // current target values - current.d = 0; + current_sp = 0; current.q = 0; + current.d = 0; //monitor_port monitor_port = nullptr; @@ -73,7 +74,7 @@ float FOCMotor::shaftVelocity() { // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print - if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); + if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); } // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! @@ -125,17 +126,17 @@ int FOCMotor::command(String user_command) { case 'I': // velocity I gain change case 'D': // velocity D gain change case 'R': // velocity voltage ramp change - if(monitor_port) monitor_port->print(" PID velocity| "); + if(monitor_port) monitor_port->print(F(" PID velocity| ")); break; case 'F': // velocity Tf low pass filter change - if(monitor_port) monitor_port->print(" LPF velocity| "); + if(monitor_port) monitor_port->print(F(" LPF velocity| ")); break; case 'K': // angle loop gain P change - if(monitor_port) monitor_port->print(" P angle| "); + if(monitor_port) monitor_port->print(F(" PID angle| ")); break; case 'L': // velocity voltage limit change case 'N': // angle loop gain velocity_limit change - if(monitor_port) monitor_port->print(" Limits| "); + if(monitor_port) monitor_port->print(F(" Limits| ")); break; } @@ -202,8 +203,12 @@ int FOCMotor::command(String user_command) { case MotionControlType::angle: if(monitor_port) monitor_port->println("angle"); break; - default: - if(monitor_port) monitor_port->println("open loop"); + case MotionControlType::velocity_openloop: + if(monitor_port) monitor_port->println("velocity openloop"); + break; + case MotionControlType::angle_openloop: + if(monitor_port) monitor_port->println("angle openloop"); + break; } }else{ // if set command switch((int)value){ @@ -219,7 +224,52 @@ int FOCMotor::command(String user_command) { if(monitor_port) monitor_port->println("angle"); controller = MotionControlType::angle; break; + case 3: + if(monitor_port) monitor_port->println("velocity openloop"); + controller = MotionControlType::velocity_openloop; + break; + case 4: + if(monitor_port) monitor_port->println("angle openloop"); + controller = MotionControlType::angle_openloop; + break; + default: // not valid command + if(monitor_port) monitor_port->println("error"); + errorFlag = 0; + } + } + break; + case 'T': + // change control type + if(monitor_port) monitor_port->print("Torque: "); + + if(GET){ // if get command + switch(torque_controller){ + case TorqueControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case TorqueControlType::current: + if(monitor_port) monitor_port->println("current"); + break; + case TorqueControlType::foc_current: + if(monitor_port) monitor_port->println("foc current"); + break; + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + torque_controller = TorqueControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("current"); + torque_controller = TorqueControlType::current; + break; + case 2: + if(monitor_port) monitor_port->println("foc current"); + torque_controller = TorqueControlType::foc_current; + break; default: // not valid command + if(monitor_port) monitor_port->println("error"); errorFlag = 0; } } diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 457dc31a..d6868013 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -117,11 +117,11 @@ class FOCMotor float shaft_angle;//!< current motor angle float electrical_angle;//!< current electrical angle float shaft_velocity;//!< current motor velocity + float current_sp;//!< target current ( q current ) float shaft_velocity_sp;//!< current target velocity float shaft_angle_sp;//!< current target angle DQVoltage_s voltage;//!< current d and q voltage set to the motor - DQCurrent_s current;//!< current d and q current set to the motor - DQCurrent_s current_measured;//!< current d and q current measured + DQCurrent_s current;//!< current d and q current measured // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter diff --git a/src/common/defaults.h b/src/common/defaults.h index 3784b8c8..80105598 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -12,12 +12,12 @@ // current sensing PID values #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // for 16Mhz controllers like Arduino uno and mega -#define DEF_PID_CURR_P 1 //!< default PID controller P value -#define DEF_PID_CURR_I 300 //!< default PID controller I value +#define DEF_PID_CURR_P 2 //!< default PID controller P value +#define DEF_PID_CURR_I 100 //!< default PID controller I value #define DEF_PID_CURR_D 0.0 //!< default PID controller D value #define DEF_PID_CURR_RAMP 1000.0 //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.005 //!< default velocity filter time constant +#define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant #else // for stm32, due, teensy, esp32 and similar #define DEF_PID_CURR_P 5 //!< default PID controller P value diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 9ea53a65..3d36b1cf 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -58,11 +58,15 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat - unsigned long i; + long i; float x, y; + // const float f = 1.5F; // better precision + + x = number * 0.5F; y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = * ( float * ) &i; + // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 610ac7db..7f6ae0f1 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -24,8 +24,9 @@ #define _PI_6 0.52359877559 #define NOT_SET -12345.0 -#define _HIGH_IMPEDANCE -1234 +#define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE +#define _ACTIVE 1 // dq current structure struct DQCurrent_s diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 8448437c..22b35795 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -17,6 +17,8 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // Inline sensor init function void InlineCurrentSense::init(){ + // configure ADC variables + _configureADC(pinA,pinB,pinC); // calibrate zero offsets calibrateOffsets(); } diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 40926165..ab376e8e 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -11,4 +11,13 @@ */ float _readADCVoltage(const int pinA); +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 708e750a..667bb8a4 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,12 +1,16 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 -#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 #define _ADC_VOLTAGE 5.0 #define _ADC_RESOLUTION 1024.0 + #ifndef cbi + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + #endif + #ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + #endif #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 @@ -20,11 +24,33 @@ #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 #else - voltage = raw_adc * 5.0/1024.0; + #define _ADC_VOLTAGE 5.0 + #define _ADC_RESOLUTION 1024.0 #endif +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + // function reading an ADC value and returning the read voltage float _readADCVoltage(const int pinA){ - int raw_adc = analogRead(pinA); - return raw_adc * (float)_ADC_VOLTAGE / (float)_ADC_RESOLUTION; + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} + + +// function reading an ADC value and returning the read voltage +void _configureADC(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( pinC != NOT_SET ) pinMode(pinC, INPUT); + + #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) + // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz + // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample + cbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); + #endif } \ No newline at end of file diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 078dd0c8..0841870d 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -63,20 +63,19 @@ int BLDCDriver3PWM::init() { } + // Set voltage to the pwm pin -void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed if(enableA_pin != NOT_SET && enableB_pin != NOT_SET && enableC_pin != NOT_SET ){ - digitalWrite(enableA_pin, Ua == _HIGH_IMPEDANCE ? LOW : HIGH); - digitalWrite(enableB_pin, Ub == _HIGH_IMPEDANCE ? LOW : HIGH); - digitalWrite(enableC_pin, Uc == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableB_pin, sb == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableC_pin, sc == _HIGH_IMPEDANCE ? LOW : HIGH); } - - // voltage of the high-impedance phase if it is not possible to disable it - // will be in exactly in the middle of the other two - if (Ua == _HIGH_IMPEDANCE) Ua = (Ub + Uc)/2; - if (Ub == _HIGH_IMPEDANCE) Ub = (Ua + Uc)/2; - if (Uc == _HIGH_IMPEDANCE) Uc = (Ua + Ub)/2; +} + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0.0, voltage_limit); diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index a785c9c8..cea4463e 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -48,6 +48,14 @@ class BLDCDriver3PWM: public BLDCDriver */ void setPwm(float Ua, float Ub, float Uc) override; + /** + * Set phase voltages to the harware + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + * @param sa - phase C state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(int sa, int sb, int sc) override; private: }; diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 33ad6800..d1a3e596 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -76,4 +76,10 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} + + +// 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 d13b69fd..68487370 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -49,6 +49,15 @@ class BLDCDriver6PWM: public BLDCDriver */ void setPwm(float Ua, float Ub, float Uc) override; + /** + * Set phase voltages to the harware + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + * @param sa - phase C state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(int sa, int sb, int sc) override; + private: }; From 267a2c932c9560377a9e7b911a598325c194f1bf Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 7 Feb 2021 09:46:34 +0100 Subject: [PATCH 076/749] update v2.1 library properties --- README.md | 4 ++-- library.properties | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index c9a5dc5b..31729d90 100644 --- a/README.md +++ b/README.md @@ -31,8 +31,8 @@ Therefore this is an attempt to: > - SVPWM full implementation d+q axis > > BEWARE 📢 slight API changes included -> - `MotionControlType` renamed into `MotionControlType` -> - `MotionControlType::voltage` does not exist any more now - `MotionControlType::torque` +> - `ControlType` renamed into `MotionControlType` +> - `ControlType::voltage` does not exist any more now - `MotionControlType::torque` ## Arduino *SimpleFOCShield* v2.0.2 diff --git a/library.properties b/library.properties index 40683407..062ff725 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.0.2 +version=2.1 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 287247aed2bf36ab4448ecee2cd21c9100c77f4d Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 10 Feb 2021 22:42:43 +0100 Subject: [PATCH 077/749] added auto calibration current sense (driver Sync) + refactoring sensor class + owen bug 6pwm --- keywords.txt | 8 +- src/BLDCMotor.cpp | 108 +++++++++++++--------- src/BLDCMotor.h | 4 +- src/StepperMotor.cpp | 62 ++++++------- src/StepperMotor.h | 2 +- src/common/base_classes/CurrentSense.cpp | 30 +++++++ src/common/base_classes/CurrentSense.h | 14 ++- src/common/base_classes/Sensor.cpp | 32 +++++++ src/common/base_classes/Sensor.h | 32 +++---- src/current_sense/InlineCurrentSense.cpp | 110 ++++++++++++++++++----- src/current_sense/InlineCurrentSense.h | 3 +- src/drivers/BLDCDriver6PWM.cpp | 2 +- src/sensors/Encoder.cpp | 41 +++------ src/sensors/Encoder.h | 20 +---- src/sensors/HallSensor.cpp | 34 +------ src/sensors/HallSensor.h | 23 ----- src/sensors/MagneticSensorAnalog.cpp | 41 +-------- src/sensors/MagneticSensorAnalog.h | 25 +----- src/sensors/MagneticSensorI2C.cpp | 43 +-------- src/sensors/MagneticSensorI2C.h | 18 +--- src/sensors/MagneticSensorSPI.cpp | 46 ++-------- src/sensors/MagneticSensorSPI.h | 19 +--- 22 files changed, 304 insertions(+), 413 deletions(-) create mode 100644 src/common/base_classes/Sensor.cpp diff --git a/keywords.txt b/keywords.txt index dccfed1b..17a72a99 100644 --- a/keywords.txt +++ b/keywords.txt @@ -69,16 +69,14 @@ ISR KEYWORD2 getVelocity KEYWORD2 setPhaseVoltage KEYWORD2 getAngle KEYWORD2 -initRelativeZero KEYWORD2 -initAbsoluteZero KEYWORD2 -hasAbsoluteZero KEYWORD2 -needsAbsoluteZeroSearch KEYWORD2 +needsSearch KEYWORD2 useMonitoring KEYWORD2 angleOpenloop KEYWORD2 velocityOpenloop KEYWORD2 -getCurrents KEYWORD2 +getPhaseCurrents KEYWORD2 getFOCCurrents KEYWORD2 getCurrent KEYWORD2 +setPwm KEYWORD2 voltage_q KEYWORD2 voltage_d KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 6a781cb7..3eefe138 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -9,7 +9,7 @@ BLDCMotor::BLDCMotor(int pp, float _R) // save pole pairs number pole_pairs = pp; // save phase resistance number - phase_resistance = _R; + phase_resistance = _R == NOT_SET ? 3.0/2.0 : _R; // torque control type is voltage by default torque_controller = TorqueControlType::voltage; } @@ -87,6 +87,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction // align motor if necessary // alignment necessary for encoders! if(zero_electric_offset != NOT_SET){ + if(monitor_port) monitor_port->println(F("MOT: Skip align.")); // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW @@ -94,16 +95,53 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction }else{ // sensor and motor alignment _delay(500); - exit_flag = alignSensor(); + if(sensor) exit_flag = alignSensor(); + else if(monitor_port) monitor_port->println(F("MOT: No sensor attached.")); + _delay(500); - } - if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); + if(current_sense) exit_flag = alignCurrentSense(); + else if(monitor_port) monitor_port->println(F("MOT: No current sense attached.")); + } + + if(exit_flag){ + if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); + }else{ + if(monitor_port) monitor_port->println(F("MOT: Calibration failed.")); + disable(); + } + + return exit_flag; +} +// Calibarthe the motor and current sense phases +int BLDCMotor::alignCurrentSense() { + int exit_flag = 1; // success + + if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); + // make sure everything is stopped + setPhaseVoltage(0, 0, 0); + + if(!current_sense->driverSync(driver, voltage_sensor_align)){ // align current sense and the driver + // error in current sense - phase either not measured or bad connection + if(monitor_port) monitor_port->println(F("MOT: Current sense align error!")); + exit_flag = 0; + } + // make sure the motor is disabled + setPhaseVoltage(0, 0, 0); + _delay(200); + return exit_flag; } + // Encoder alignment to electrical 0 angle int BLDCMotor::alignSensor() { + int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + + // check if sensor needs zero search + if(sensor->needsSearch()) absoluteZeroSearch(); + _delay(500); + // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); @@ -123,64 +161,50 @@ int BLDCMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - // determin the direction the sensor moved + // determine the direction the sensor moved if (mid_angle < start_angle) { if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); sensor->natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); + exit_flag = 0; // failed calibration } else{ if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); } - // let the motor stabilize for1 sec + // let the motor stabilize for 1 sec _delay(1000); // set sensor to zero - sensor->initRelativeZero(); + zero_electric_angle = _normalizeAngle(_electricalAngle(shaftAngle(), pole_pairs)); _delay(500); setPhaseVoltage(0, 0, 0); _delay(200); - - // find the index if available - int exit_flag = absoluteZeroAlign(); - _delay(500); - if(monitor_port){ - if(exit_flag< 0 ) monitor_port->println(F("MOT: Error: Not found!")); - if(exit_flag> 0 ) monitor_port->println(F("MOT: Success!")); - else monitor_port->println(F("MOT: Not available!")); - } + return exit_flag; } // Encoder alignment the absolute zero angle // - to the index -int BLDCMotor::absoluteZeroAlign() { - - if(monitor_port) monitor_port->println(F("MOT: Absolute zero align.")); - // if no absolute zero return - if(!sensor->hasAbsoluteZero()) return 0; - - - if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println(F("MOT: Searching...")); +void BLDCMotor::absoluteZeroSearch() { + + if(monitor_port) monitor_port->println(F("MOT: Absolute zero search...")); // search the absolute zero with small velocity - while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ - loopFOC(); - voltage.q = PID_velocity(velocity_index_search - shaftVelocity()); + float limit = velocity_limit; + velocity_limit = velocity_index_search; + shaft_angle = 0; + while(sensor->needsSearch() && shaft_angle < _2PI){ + angleOpenloop(1.5*_2PI); } - voltage.q = 0; // disable motor setPhaseVoltage(0, 0, 0); - - // align absolute zero if it has been found - if(!sensor->needsAbsoluteZeroSearch()){ - // align the sensor with the absolute zero - float zero_offset = sensor->initAbsoluteZero(); - // remember zero electric angle - zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + // reinit the limits + velocity_limit = limit; + // check if the zero found + if(monitor_port){ + if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); + else monitor_port->println(F("MOT: Success!")); } - // return bool if zero found - return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; } // Iterative function looping FOC algorithm, setting Uq on the Motor @@ -191,13 +215,14 @@ void BLDCMotor::loopFOC() { // shaft angle shaft_angle = shaftAngle(); - electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs) + zero_electric_angle); + electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs) - zero_electric_angle); switch (torque_controller) { case TorqueControlType::voltage: // no need to do anything really break; case TorqueControlType::current: + if(!current_sense) return; // read overall current magnitude current.q = current_sense->getCurrent(electrical_angle); // filter the value values @@ -207,6 +232,7 @@ void BLDCMotor::loopFOC() { voltage.d = 0; break; case TorqueControlType::foc_current: + if(!current_sense) return; // read dq currents current = current_sense->getFOCCurrents(electrical_angle); // filter values @@ -254,7 +280,8 @@ void BLDCMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command if(torque_controller == TorqueControlType::voltage){ - voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; }else{ current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control @@ -265,7 +292,8 @@ void BLDCMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command if(torque_controller == TorqueControlType::voltage){ - voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; }else{ current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 147b6456..af1eb2b6 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -83,8 +83,10 @@ class BLDCMotor: public FOCMotor void setPhaseVoltage(float Uq, float Ud, float angle_el); /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); + /** Current sense and motor phase alignment */ + int alignCurrentSense(); /** Motor and sensor alignment to the sensors absolute 0 angle */ - int absoluteZeroAlign(); + void absoluteZeroSearch(); // Open loop motion control diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2705692b..256acc9a 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -87,9 +87,15 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_directi return exit_flag; } + // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + + // check if sensor needs zero search + if(sensor->needsSearch()) absoluteZeroSearch(); + _delay(500); + // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees float start_angle = shaftAngle(); @@ -109,7 +115,7 @@ int StepperMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - // determin the direction the sensor moved + // determine the direction the sensor moved if (mid_angle < start_angle) { if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); sensor->natural_direction = Direction::CCW; @@ -119,55 +125,37 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); } - // let the motor stabilize for1 sec + // let the motor stabilize for 1 sec _delay(1000); // set sensor to zero - sensor->initRelativeZero(); + zero_electric_angle = _normalizeAngle(_electricalAngle(shaftAngle(), pole_pairs)); _delay(500); setPhaseVoltage(0, 0, 0); _delay(200); - - // find the index if available - int exit_flag = absoluteZeroAlign(); - _delay(500); - if(monitor_port){ - if(exit_flag< 0 ) monitor_port->println(F("MOT: Error: Not found!")); - if(exit_flag> 0 ) monitor_port->println(F("MOT: Success!")); - else monitor_port->println(F("MOT: Not available!")); - } - return exit_flag; + + return 0; } - -// Encoder alignment the absolute zero angle +// Encoder alignment the absolute zero angle // - to the index -int StepperMotor::absoluteZeroAlign() { - - if(monitor_port) monitor_port->println(F("MOT: Absolute zero align.")); - // if no absolute zero return - if(!sensor->hasAbsoluteZero()) return 0; +void StepperMotor::absoluteZeroSearch() { - - if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println(F("MOT: Searching...")); + if(monitor_port) monitor_port->println(F("MOT: Absolute zero search...")); // search the absolute zero with small velocity - while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ - loopFOC(); - voltage.q = PID_velocity(velocity_index_search - shaftVelocity()); + float limit = velocity_limit; + velocity_limit = velocity_index_search; + while(sensor->needsSearch() && shaft_angle < _2PI){ + angleOpenloop(1.5*_2PI); } - voltage.q = 0; - voltage.d = 0; // disable motor setPhaseVoltage(0, 0, 0); - - // align absolute zero if it has been found - if(!sensor->needsAbsoluteZeroSearch()){ - // align the sensor with the absolute zero - float zero_offset = sensor->initAbsoluteZero(); - // remember zero electric angle - zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + // reinit the limits + velocity_limit = limit; + // check if the zero found + if(monitor_port){ + if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); + else monitor_port->println(F("MOT: Success!")); } - // return bool if zero found - return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; } // Iterative function looping FOC algorithm, setting Uq on the Motor @@ -177,7 +165,7 @@ void StepperMotor::loopFOC() { if(!enabled) return; // shaft angle shaft_angle = shaftAngle(); - electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle, pole_pairs) + zero_electric_angle); + electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle, pole_pairs) - zero_electric_angle); // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index af4783bf..6993c68f 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -92,7 +92,7 @@ class StepperMotor: public FOCMotor /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ - int absoluteZeroAlign(); + void absoluteZeroSearch(); // Open loop motion control /** diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index caa3b572..b21a72a4 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -1,5 +1,35 @@ #include "CurrentSense.h" + +// get current magnitude +// - absolute - if no electrical_angle provided +// - signed - if angle provided +float CurrentSense::getCurrent(float motor_electrical_angle){ + // read current phase currents + PhaseCurrent_s current = getPhaseCurrents(); + // currnet sign - if motor angle not provided the magnitude is always positive + float sign = 1; + + // calculate clarke transform + float i_alpha, i_beta; + if(!current.c){ + // if only two measured currents + 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 ); + } + + // if motor angle provided function returns signed value of the current + // determine the sign of the current + // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 + if(motor_electrical_angle) + sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; + // return current magnitude + return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); +} + // function used with the foc algorihtm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 34e15780..d894ad34 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,7 +1,7 @@ #ifndef CURRENTSENSE_H #define CURRENTSENSE_H - +#include "BLDCDriver.h" #include "../foc_utils.h" /** @@ -15,7 +15,15 @@ class CurrentSense{ * Function intialising the CurrentSense class * All the necessary intialisations of adc and sync should be implemented here */ - virtual void init(); + virtual void init() = 0; + + /** + * Function intended to implement all that is needed to sync and calibrate the current sensing with the driver. + * If no such thing is needed it can be left empty (return 1) + * @returns - 0 - for failure & 1 - for success + */ + virtual int driverSync(BLDCDriver *driver, float voltage) = 0; + /** * Function rading the phase currents a, b and c * This function will be used with the foc control throught the function @@ -24,7 +32,7 @@ class CurrentSense{ * * @return PhaseCurrent_s current values */ - virtual PhaseCurrent_s getPhaseCurrents(); + virtual PhaseCurrent_s getPhaseCurrents() = 0; /** * Function reading the magnitude of the current set to the motor * It returns the abosolute or signed magnitude if possible diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp new file mode 100644 index 00000000..fba66907 --- /dev/null +++ b/src/common/base_classes/Sensor.cpp @@ -0,0 +1,32 @@ +#include "Sensor.h" +#include "../foc_utils.h" +#include "../time_utils.h" + + /** + * returns 0 if it does need search for absolute zero + * 0 - magnetic sensor (& encoder with index which is found) + * 1 - ecoder with index (with index not found yet) + */ +int Sensor::needsSearch(){ + return 0; +} + + /** get current angular velocity (rad/s)*/ +float Sensor::getVelocity(){ + + // calculate sample time + unsigned long now_us = _micros(); + float Ts = (now_us - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // current angle + float angle_c = getAngle(); + // velocity calculation + float vel = (angle_c - angle_prev)/Ts; + + // save variables for future pass + angle_prev = angle_c; + velocity_calc_timestamp = now_us; + return vel; +} \ No newline at end of file diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 7370c38f..51b91e99 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -24,36 +24,26 @@ enum Pullup{ */ class Sensor{ public: - /** get current angle (rad) */ - virtual float getAngle(); - /** get current angular velocity (rad/s)*/ - virtual float getVelocity(); - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - virtual float initRelativeZero(); - /** - * set absolute zero angle as zero angle - * return the angle [rad] difference - */ - virtual float initAbsoluteZero(); // if natural_direction == Direction::CCW then direction will be flipped to CW int natural_direction = Direction::CW; + float zero_offset = 0; //!< user defined zero offset + + /** get current angle (rad) */ + virtual float getAngle()=0; + /** get current angular velocity (rad/s)*/ + virtual float getVelocity(); - /** - * returns 0 if it has no absolute 0 measurement - * 0 - incremental encoder without index - * 1 - encoder with index & magnetic sensors - */ - virtual int hasAbsoluteZero(); /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) */ - virtual int needsAbsoluteZeroSearch(); + virtual int needsSearch(); + private: + // velocity calculation variables + float angle_prev=0; //!< angle in previous velocity calculation step + long velocity_calc_timestamp=0; //!< last velocity calculation timestamp }; #endif \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 22b35795..6f5b5d51 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -49,31 +49,95 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ return current; } -// get current magnitude -// - absolute - if no electrical_angle provided -// - signed - if angle provided -float InlineCurrentSense::getCurrent(float motor_electrical_angle){ - // read current phase currents - PhaseCurrent_s current = getPhaseCurrents(); - // currnet sign - if motor angle not provided the magnitude is always positive - float sign = 1; +// Function synchronizing and aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns 0 for failure and 1 for success +int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ - // calculate clarke transform - float i_alpha, i_beta; - if(!current.c){ - // if only two measured currents - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(1000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = fabs(c.a / c.b); + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if( ab_ratio > 1.5 ){ // should be ~2 + gain_adjust_a = _sign(c.a); + }else if( ab_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + gain_adjust_a = _sign(c.b); + }else if(pinC != NOT_SET && ac_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + gain_adjust_a = _sign(c.c); + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(1000); + c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = fabs(c.b/c.a); + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if( ba_ratio > 1.5 ){ // should be ~2 + gain_adjust_b = _sign(c.b); + }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + gain_adjust_b = _sign(c.a); + }else if(pinC != NOT_SET && bc_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + gain_adjust_b = _sign(c.c); }else{ - i_alpha = 2*(current.a - (current.b - current.c))/3.0; - i_beta = _2_SQRT3 *( current.b - current.c ); + // error in current sense - phase either not measured or bad connection + return 0; } - // if motor angle provided function returns signed value of the current - // determine the sign of the current - // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 - if(motor_electrical_angle) - sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; - // return current magnitude - return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); + // if phase C measured + if(pinC != NOT_SET){ + // set phase B active and phases A and C down + driver->setPwm(0, 0, voltage); + _delay(200); + c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 50; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.c = (c.c+c1.c)/50.0; + } + driver->setPwm(0, 0, 0); + gain_adjust_c = _sign(c.c); + } + + return 1; } + diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 3c9323c6..3ff645ae 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -23,8 +23,7 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions void init() override; PhaseCurrent_s getPhaseCurrents() override; - float getCurrent( float angle_el = 0) override; - + int driverSync(BLDCDriver *driver, float voltage) override; // ADC measuremnet gain adjustment for each phase // if (shunt+amp combination) measures inverse current you can set it to -1 for example diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index d1a3e596..2d9cb256 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -45,7 +45,7 @@ int BLDCDriver6PWM::init() { _delay(1000); // PWM pins - pinMode(pwmA_l, OUTPUT); + pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); pinMode(pwmC_h, OUTPUT); pinMode(pwmA_l, OUTPUT); diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 1965c015..89daaba9 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -24,7 +24,6 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ I_active = 0; // index pin index_pin = _index; // its 0 if not used - index_pulse_counter = 0; // velocity calculation variables prev_Th = 0; @@ -87,17 +86,13 @@ void Encoder::handleIndex() { if(hasIndex()){ int I = digitalRead(index_pin); if(I && !I_active){ + index_found = true; // align encoder on each index - if(index_pulse_counter){ - long tmp = pulse_counter; - // corrent the counter value - pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; - // preserve relative speed - prev_pulse_counter += pulse_counter - tmp; - } else { - // initial offset - index_pulse_counter = pulse_counter; - } + long tmp = pulse_counter; + // corrent the counter value + pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; + // preserve relative speed + prev_pulse_counter += pulse_counter - tmp; } I_active = I; } @@ -107,7 +102,7 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getAngle(){ - return natural_direction * _2PI * (pulse_counter) / ((float)cpr); + return natural_direction * _2PI * (pulse_counter) / ((float)cpr) - zero_offset; } /* Shaft velocity calculation @@ -150,26 +145,10 @@ float Encoder::getVelocity(){ // getter for index pin // return -1 if no index -int Encoder::needsAbsoluteZeroSearch(){ - return index_pulse_counter == 0; -} -// getter for index pin -int Encoder::hasAbsoluteZero(){ - return hasIndex(); -} -// initialize counter to zero -float Encoder::initRelativeZero(){ - long angle_offset = -pulse_counter; - pulse_counter = 0; - pulse_timestamp = _micros(); - return _2PI * (angle_offset) / ((float)cpr); -} -// initialize index to zero -float Encoder::initAbsoluteZero(){ - pulse_counter -= index_pulse_counter; - prev_pulse_counter = pulse_counter; - return (index_pulse_counter) / ((float)cpr) * (_2PI); +int Encoder::needsSearch(){ + return hasIndex() && !index_found; } + // private function used to determine if encoder has index int Encoder::hasIndex(){ return index_pin != 0; diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 29122888..7a0f7a40 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -63,28 +63,12 @@ class Encoder: public Sensor{ float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - float initRelativeZero() override; - /** - * set index angle as zero angle - * return the angle [rad] difference - */ - float initAbsoluteZero() override; - /** - * returns 0 if it has no index - * 0 - encoder without index - * 1 - encoder with index - */ - int hasAbsoluteZero() override; /** * returns 0 if it does need search for absolute zero * 0 - encoder without index * 1 - ecoder with index */ - int needsAbsoluteZeroSearch() override; + int needsSearch() override; private: int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not. @@ -94,7 +78,7 @@ class Encoder: public Sensor{ volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel volatile int I_active; //!< current active states of Index channel - volatile long index_pulse_counter; //!< impulse counter number upon first index interrupt + volatile bool index_found = false; //!< flag stating that the index has been found // velocity calculation variables float prev_Th, pulse_per_second; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 8cf08a0f..2e3ecefe 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,7 +96,7 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { Shaft angle calculation */ float HallSensor::getAngle() { - return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI; + return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI - zero_offset; } /* @@ -112,38 +112,12 @@ float HallSensor::getVelocity(){ } -// getter for index pin -// return -1 if no index -int HallSensor::needsAbsoluteZeroSearch(){ - return 0; -} - -int HallSensor::hasAbsoluteZero(){ - return 1; -} - -// set current angle as zero angle -// return the angle [rad] difference -float HallSensor::initRelativeZero(){ - - // nothing to do. The interrupts should have changed sector. - electric_rotations = 0; - return 0; - -} - -// set absolute zero angle as zero angle -// return the angle [rad] difference -float HallSensor::initAbsoluteZero(){ - - return -getAngle(); - -} - // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ - + // initialise the electrical rotations to 0 + electric_rotations = 0; + // HallSensor - check if pullup needed for your HallSensor if(pullup == Pullup::INTERN){ pinMode(pinA, INPUT_PULLUP); diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 5653faf2..167041ce 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -57,28 +57,6 @@ class HallSensor: public Sensor{ float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - float initRelativeZero() override; - /** - * set index angle as zero angle - * return the angle [rad] difference - */ - float initAbsoluteZero() override; - /** - * returns 0 if it has no index - * 0 - HallSensor without index - * 1 - HallSensor with index - */ - int hasAbsoluteZero() override; - /** - * returns 0 if it does need search for absolute zero - * 0 - HallSensor without index - * 1 - ecoder with index - */ - int needsAbsoluteZeroSearch() override; // whether last step was CW (+1) or CCW (-1). Note - this is a raw direction (i.e. doesn't include natural_direction reversal) Direction direction; @@ -99,7 +77,6 @@ class HallSensor: public Sensor{ Direction decodeDirection(int oldState, int newState); void updateState(); - int zero_offset; volatile long pulse_timestamp;//!< last impulse timestamp in us volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index ed112a94..a1fd207f 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -31,7 +31,6 @@ void MagneticSensorAnalog::init(){ // full rotations tracking number full_rotation_offset = 0; raw_count_prev = getRawCount(); - zero_offset = 0; } // Shaft angle calculation @@ -43,8 +42,8 @@ float MagneticSensorAnalog::getAngle(){ 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 = natural_direction * (full_rotation_offset + ( (float) (raw_count - zero_offset) / (float)cpr) * _2PI); + + float angle = natural_direction * (full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI) - zero_offset; // calculate velocity here long now = _micros(); @@ -67,42 +66,6 @@ float MagneticSensorAnalog::getVelocity(){ return velocity; } -// set current angle as zero angle -// return the angle [rad] difference -float MagneticSensorAnalog::initRelativeZero(){ - - float angle_offset = -getAngle(); - zero_offset = natural_direction * getRawCount(); - - // angle tracking variables - full_rotation_offset = 0; - return angle_offset; -} -// set absolute zero angle as zero angle -// return the angle [rad] difference -float MagneticSensorAnalog::initAbsoluteZero(){ - float rotation = -(int)zero_offset; - // init absolute zero - zero_offset = 0; - - // angle tracking variables - full_rotation_offset = 0; - // return offset in radians - return rotation / (float)cpr * _2PI; -} -// returns 0 if it has no absolute 0 measurement -// 0 - incremental encoder without index -// 1 - encoder with index & magnetic sensors -int MagneticSensorAnalog::hasAbsoluteZero(){ - return 1; -} -// returns 0 if it does need search for absolute zero -// 0 - magnetic sensor -// 1 - ecoder with index -int MagneticSensorAnalog::needsAbsoluteZeroSearch(){ - return 0; -} - // function reading the raw counter of the magnetic sensor int MagneticSensorAnalog::getRawCount(){ return analogRead(pinAnalog); diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index d76b6eb4..33d5b25c 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -26,39 +26,22 @@ class MagneticSensorAnalog: public Sensor{ // Encoder configuration Pullup pullup; - + // implementation of abstract functions of the Sensor class /** get current angle (rad) */ float getAngle() override; /** get current angular velocity (rad/s) **/ float getVelocity() override; - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - float initRelativeZero() override; - /** - * set absolute zero angle as zero angle - * return the angle [rad] difference - */ - float initAbsoluteZero() override; - /** returns 1 because it is the absolute sensor */ - int hasAbsoluteZero() override; - /** returns 0 maning it doesn't need search for absolute zero */ - int needsAbsoluteZeroSearch() override; + + + private: /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; int min_raw_count; int max_raw_count; int cpr; - - private: - // float cpr; //!< Maximum range of the magnetic sensor - - int read(); - int zero_offset; //!< user defined zero offset /** * Function getting current angle register value * it uses angle_register variable diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index accce08a..b07fb4b2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -72,7 +72,6 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // full rotations tracking number full_rotation_offset = 0; angle_data_prev = getRawCount(); - zero_offset = 0; } // Shaft angle calculation @@ -90,12 +89,9 @@ float MagneticSensorI2C::getAngle(){ // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - - // zero offset adding - angle_data -= (int)zero_offset; // return the full angle - // (number of full rotations)*2PI + current sensor angle - return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); + // (number of full rotations)*2PI + current sensor angle - zero_offset + return (natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) - zero_offset); } // Shaft velocity calculation @@ -117,41 +113,6 @@ float MagneticSensorI2C::getVelocity(){ return vel; } -// set current angle as zero angle -// return the angle [rad] difference -float MagneticSensorI2C::initRelativeZero(){ - float angle_offset = -getAngle(); - zero_offset = natural_direction * getRawCount(); - - // angle tracking variables - full_rotation_offset = 0; - return angle_offset; -} -// set absolute zero angle as zero angle -// return the angle [rad] difference -float MagneticSensorI2C::initAbsoluteZero(){ - float rotation = -(int)zero_offset; - // init absolute zero - zero_offset = 0; - - // angle tracking variables - full_rotation_offset = 0; - // return offset in radians - return rotation / (float)cpr * _2PI; -} -// returns 0 if it has no absolute 0 measurement -// 0 - incremental encoder without index -// 1 - encoder with index & magnetic sensors -int MagneticSensorI2C::hasAbsoluteZero(){ - return 1; -} -// returns 0 if it does need search for absolute zero -// 0 - magnetic sensor -// 1 - ecoder with index -int MagneticSensorI2C::needsAbsoluteZeroSearch(){ - return 0; -} - // function reading the raw counter of the magnetic sensor int MagneticSensorI2C::getRawCount(){ diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 7fe4efeb..6ff2c761 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -34,7 +34,7 @@ class MagneticSensorI2C: public Sensor{ MagneticSensorI2C(MagneticSensorI2CConfig_s config); static MagneticSensorI2C AS5600(); - + /** sensor initialise pins */ void init(TwoWire* _wire = &Wire); @@ -43,21 +43,6 @@ class MagneticSensorI2C: public Sensor{ float getAngle() override; /** get current angular velocity (rad/s) **/ float getVelocity() override; - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - float initRelativeZero() override; - /** - * set absolute zero angle as zero angle - * return the angle [rad] difference - */ - float initAbsoluteZero() override; - /** returns 1 because it is the absolute sensor */ - int hasAbsoluteZero() override; - /** returns 0 maning it doesn't need search for absolute zero */ - - int needsAbsoluteZeroSearch() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); @@ -76,7 +61,6 @@ class MagneticSensorI2C: public Sensor{ /** Read one I2C register value */ int read(uint8_t angle_register_msb); - word zero_offset; //!< user defined zero offset /** * Function getting current angle register value * it uses angle_register variable diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 58660e0b..8a842098 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -10,6 +10,9 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .command_rw_bit = 14, .command_parity_bit = 15 }; +// AS5048 and AS5047 are the same as AS5147 +MagneticSensorSPIConfig_s AS5048_SPI = AS5147_SPI; +MagneticSensorSPIConfig_s AS5047_SPI = AS5147_SPI; /** Typical configuration for the 14bit MonolithicPower MA730 magnetic sensor over SPI interface */ MagneticSensorSPIConfig_s MA730_SPI = { @@ -41,7 +44,6 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r command_parity_bit = 15; // for backwards compatibilty command_rw_bit = 14; // for backwards compatibilty data_start_bit = 13; // for backwards compatibilty - } MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ @@ -85,7 +87,6 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // full rotations tracking number full_rotation_offset = 0; angle_data_prev = getRawCount(); - zero_offset = 0; } // Shaft angle calculation @@ -104,11 +105,9 @@ float MagneticSensorSPI::getAngle(){ // in order to know if overflow happened angle_data_prev = angle_data; - // zero offset adding - angle_data -= (int)zero_offset; // return the full angle - // (number of full rotations)*2PI + current sensor angle - return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); + // (number of full rotations)*2PI + current sensor angle - zero_offset + return (natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) - zero_offset); } // Shaft velocity calculation @@ -130,41 +129,6 @@ float MagneticSensorSPI::getVelocity(){ return vel; } -// set current angle as zero angle -// return the angle [rad] difference -float MagneticSensorSPI::initRelativeZero(){ - float angle_offset = -getAngle(); - zero_offset = natural_direction * getRawCount(); - - // angle tracking variables - full_rotation_offset = 0; - return angle_offset; -} -// set absolute zero angle as zero angle -// return the angle [rad] difference -float MagneticSensorSPI::initAbsoluteZero(){ - float rotation = -(int)zero_offset; - // init absolute zero - zero_offset = 0; - - // angle tracking variables - full_rotation_offset = 0; - // return offset in radians - return rotation / (float)cpr * _2PI; -} -// returns 0 if it has no absolute 0 measurement -// 0 - incremental encoder without index -// 1 - encoder with index & magnetic sensors -int MagneticSensorSPI::hasAbsoluteZero(){ - return 1; -} -// returns 0 if it does need search for absolute zero -// 0 - magnetic sensor -// 1 - ecoder with index -int MagneticSensorSPI::needsAbsoluteZeroSearch(){ - return 0; -} - // function reading the raw counter of the magnetic sensor int MagneticSensorSPI::getRawCount(){ diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 05fb9091..764053df 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -19,7 +19,7 @@ struct MagneticSensorSPIConfig_s { int command_parity_bit; }; // typical configuration structures -extern MagneticSensorSPIConfig_s AS5147_SPI, MA730_SPI; +extern MagneticSensorSPIConfig_s AS5147_SPI,AS5048_SPI,AS5047_SPI, MA730_SPI; class MagneticSensorSPI: public Sensor{ public: @@ -45,21 +45,6 @@ class MagneticSensorSPI: public Sensor{ float getAngle() override; /** get current angular velocity (rad/s) **/ float getVelocity() override; - /** - * set current angle as zero angle - * return the angle [rad] difference - */ - float initRelativeZero() override; - /** - * set absolute zero angle as zero angle - * return the angle [rad] difference - */ - float initAbsoluteZero() override; - /** returns 1 because it is the absolute sensor */ - int hasAbsoluteZero() override; - /** returns 0 maning it doesn't need search for absolute zero */ - - int needsAbsoluteZeroSearch() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; @@ -82,8 +67,6 @@ class MagneticSensorSPI: public Sensor{ /** Calculate parity value */ byte spiCalcEvenParity(word value); - - word zero_offset; //!< user defined zero offset /** * Function getting current angle register value * it uses angle_register variable From b501d6d0dff1898ee5bdb44ddaeeabf5688390cd Mon Sep 17 00:00:00 2001 From: Sergey Royz Date: Thu, 11 Feb 2021 00:38:41 +0100 Subject: [PATCH 078/749] dual h-bridge v1.3 support --- src/SimpleFOC.h | 1 + src/drivers/StepperDriver2PWM2Pin.cpp | 88 +++++++++++++++++++++++++++ src/drivers/StepperDriver2PWM2Pin.h | 55 +++++++++++++++++ 3 files changed, 144 insertions(+) create mode 100644 src/drivers/StepperDriver2PWM2Pin.cpp create mode 100644 src/drivers/StepperDriver2PWM2Pin.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 27b9ab84..6edc6784 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -107,6 +107,7 @@ void loop() { #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" +#include "drivers/StepperDriver2PWM2Pin.h" #include "current_sense/InlineCurrentSense.h" #endif diff --git a/src/drivers/StepperDriver2PWM2Pin.cpp b/src/drivers/StepperDriver2PWM2Pin.cpp new file mode 100644 index 00000000..eb68e495 --- /dev/null +++ b/src/drivers/StepperDriver2PWM2Pin.cpp @@ -0,0 +1,88 @@ +#include "StepperDriver2PWM2Pin.h" + +StepperDriver2PWM2Pin::StepperDriver2PWM2Pin(int ph1PWM, int ph1Dir,int ph2PWM, int ph2Dir, int en1, int en2) { + // Pin initialization + pwm1 = ph1PWM; //!< phase 1 pwm pin number + dir1 = ph1Dir; //!< phase 1 dir pin number + pwm2 = ph2PWM; //!< phase 2 pwm pin number + dir2 = ph2Dir; //!< phase 2 dir pin number + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver2PWM2Pin::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver2PWM2Pin::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +int StepperDriver2PWM2Pin::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + pinMode(dir1, OUTPUT); + pinMode(dir2, OUTPUT); + + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure2PWM(pwm_frequency, pwm1, pwm2); + return 0; +} + + +// Set voltage to the pwm pin +void StepperDriver2PWM2Pin::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1(0.0),duty_cycle2(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + // hardware specific writing + duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + // set direction + digitalWrite(dir2, Ualpha > 0 ? HIGH : LOW); + digitalWrite(dir1, Ubeta > 0 ? HIGH : LOW); + + _delay(1); + // Serial.print(Ualpha); + // Serial.print("\t"); + // Serial.print(Ubeta); + // Serial.print("\t"); + // Serial.print(duty_cycle1); + // Serial.print("\t"); + // Serial.println(duty_cycle2); + + // write to hardware + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); +} \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM2Pin.h b/src/drivers/StepperDriver2PWM2Pin.h new file mode 100644 index 00000000..8b6e17aa --- /dev/null +++ b/src/drivers/StepperDriver2PWM2Pin.h @@ -0,0 +1,55 @@ +#ifndef STEPPER_DRIVER_2PWM_2PIN_h +#define STEPPER_DRIVER_2PWM_2PIN_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 2 pwm stepper driver class +*/ +class StepperDriver2PWM2Pin: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1PWM PWM1 phase pwm pin + @param ph1Dir DIR1 phase dir pin + @param ph2PWM PWM2 phase pwm pin + @param ph2Dir DIR2 phase dir pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver2PWM2Pin(int ph1PWM, int ph1Dir,int ph2PWM, int ph2Dir, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1; //!< phase 1 pwm pin number + int dir1; //!< phase 1 dir pin number + int pwm2; //!< phase 2 pwm pin number + int dir2; //!< phase 2 dir pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif From da9627fc481f61ecb4b32558b8dfb6fbda83ec32 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 11 Feb 2021 17:51:36 +0100 Subject: [PATCH 079/749] Updated StepperDriver2PWM to include hte changes --- src/drivers/StepperDriver2PWM.cpp | 79 +++++++++++++----------- src/drivers/StepperDriver2PWM.h | 33 ++++++---- src/drivers/StepperDriver2PWM2Pin.cpp | 88 --------------------------- src/drivers/StepperDriver2PWM2Pin.h | 55 ----------------- 4 files changed, 66 insertions(+), 189 deletions(-) delete mode 100644 src/drivers/StepperDriver2PWM2Pin.cpp delete mode 100644 src/drivers/StepperDriver2PWM2Pin.h diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 72182648..a08c5e6b 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -1,13 +1,33 @@ #include "StepperDriver2PWM.h" -StepperDriver2PWM::StepperDriver2PWM(int ph1PWM, int ph1INA, int ph1INB, int ph2PWM, int ph2INA, int ph2INB, int en1, int en2){ +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2){ // Pin initialization - pwm1 = ph1PWM; //!< phase 1 pwm pin number - ina1 = ph1INA; //!< phase 1 INA pin number - inb1 = ph1INB; //!< phase 1 INB pin number - pwm2 = ph2PWM; //!< phase 2 pwm pin number - ina2 = ph2INA; //!< phase 2 INA pin number - inb2 = ph2INB; //!< phase 2 INB pin number + pwm1 = _pwm1; // phase 1 pwm pin number + dir1a = _in1a; // phase 1 INA pin number + dir1b = _in1b; // phase 1 INB pin number + pwm2 = _pwm2; // phase 2 pwm pin number + dir2a = _in2a; // phase 2 INA pin number + dir2b = _in2b; // phase 2 INB pin number + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, int en1, int en2){ + // Pin initialization + pwm1 = _pwm1; // phase 1 pwm pin number + dir1a = _dir1; // phase 1 direction pin + pwm2 = _pwm2; // phase 2 pwm pin number + dir2a = _dir2; // phase 2 direction pin + // these pins are not used + dir1b = NOT_SET; + dir2b = NOT_SET; // enable_pin pin enable_pin1 = en1; @@ -47,10 +67,10 @@ int StepperDriver2PWM::init() { // PWM pins pinMode(pwm1, OUTPUT); pinMode(pwm2, OUTPUT); - pinMode(ina1, OUTPUT); - pinMode(ina2, OUTPUT); - pinMode(inb1, OUTPUT); - pinMode(inb2, OUTPUT); + pinMode(dir1a, OUTPUT); + pinMode(dir2a, OUTPUT); + if(dir1b != NOT_SET ) pinMode(dir1b, OUTPUT); + if(dir2b != NOT_SET ) pinMode(dir2b, OUTPUT); if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); @@ -66,33 +86,22 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ualpha, float Ubeta) { +void StepperDriver2PWM::setPwm(float Ua, float Ub) { float duty_cycle1(0.0),duty_cycle2(0.0); // limit the voltage in driver - Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); - Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + Ua = _constrain(Ua, -voltage_limit, voltage_limit); + Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); - // set direction - if( Ualpha > 0 ){ - digitalWrite(inb1, LOW); - digitalWrite(ina1, HIGH); - } - else{ - digitalWrite(ina1, LOW); - digitalWrite(inb1, HIGH); - } - - if( Ubeta > 0 ){ - digitalWrite(ina2, LOW); - digitalWrite(inb2, HIGH); - } - else{ - digitalWrite(inb2, LOW); - digitalWrite(ina2, HIGH); - } - + duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); + duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); + + // phase 1 direction + digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); + if(dir1b != NOT_SET) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + // phase 2 direction + digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); + if(dir2b != NOT_SET) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); + // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); } \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index ef8797ee..cab796d3 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -15,17 +15,28 @@ class StepperDriver2PWM: public StepperDriver public: /** StepperMotor class constructor - @param ph1PWM PWM1 phase pwm pin - @param ph1INA IN1A phase dir pin - @param ph1INB IN1B phase dir pin - @param ph2PWM PWM2 phase pwm pin - @param ph2INA IN2A phase dir pin - @param ph2INB IN2B phase dir pin + @param pwm1 PWM1 phase pwm pin + @param in1a IN1A phase dir pin + @param in1b IN1B phase dir pin + @param pwm2 PWM2 phase pwm pin + @param in2a IN2A phase dir pin + @param in2b IN2B phase dir pin @param en1 enable pin phase 1 (optional input) @param en2 enable pin phase 2 (optional input) */ - StepperDriver2PWM(int ph1PWM,int ph1INA,int ph1INB,int ph2PWM,int ph2INA,int ph2INB, int en1 = NOT_SET, int en2 = NOT_SET); + StepperDriver2PWM(int pwm1, int in1a, int in1b, int pwm2, int in2a, int in2b, int en1 = NOT_SET, int en2 = NOT_SET); + /** + StepperMotor class constructor + @param pwm1 PWM1 phase pwm pin + @param dir1 DIR1 phase dir pin + @param pwm2 PWM2 phase pwm pin + @param dir2 DIR2 phase dir pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver2PWM(int pwm1, int dir1, int pwm2, int dir2, int en1 = NOT_SET, int en2 = NOT_SET); + /** Motor hardware init function */ int init() override; /** Motor disable function */ @@ -35,11 +46,11 @@ class StepperDriver2PWM: public StepperDriver // hardware variables int pwm1; //!< phase 1 pwm pin number - int ina1; //!< phase 1 INA pin number - int inb1; //!< phase 1 INB pin number + int dir1a; //!< phase 1 INA pin number + int dir1b; //!< phase 1 INB pin number int pwm2; //!< phase 2 pwm pin number - int ina2; //!< phase 2 INA pin number - int inb2; //!< phase 2 INB pin number + int dir2a; //!< phase 2 INA pin number + int dir2b; //!< phase 2 INB pin number int enable_pin1; //!< enable pin number phase 1 int enable_pin2; //!< enable pin number phase 2 diff --git a/src/drivers/StepperDriver2PWM2Pin.cpp b/src/drivers/StepperDriver2PWM2Pin.cpp deleted file mode 100644 index eb68e495..00000000 --- a/src/drivers/StepperDriver2PWM2Pin.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "StepperDriver2PWM2Pin.h" - -StepperDriver2PWM2Pin::StepperDriver2PWM2Pin(int ph1PWM, int ph1Dir,int ph2PWM, int ph2Dir, int en1, int en2) { - // Pin initialization - pwm1 = ph1PWM; //!< phase 1 pwm pin number - dir1 = ph1Dir; //!< phase 1 dir pin number - pwm2 = ph2PWM; //!< phase 2 pwm pin number - dir2 = ph2Dir; //!< phase 2 dir pin number - - // enable_pin pin - enable_pin1 = en1; - enable_pin2 = en2; - - // default power-supply value - voltage_power_supply = DEF_POWER_SUPPLY; - voltage_limit = NOT_SET; - -} - -// enable motor driver -void StepperDriver2PWM2Pin::enable(){ - // enable_pin the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); - // set zero to PWM - setPwm(0,0); -} - -// disable motor driver -void StepperDriver2PWM2Pin::disable() -{ - // set zero to PWM - setPwm(0, 0); - // disable the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); - -} - -// init hardware pins -int StepperDriver2PWM2Pin::init() { - // a bit of separation - _delay(1000); - - // PWM pins - pinMode(pwm1, OUTPUT); - pinMode(pwm2, OUTPUT); - pinMode(dir1, OUTPUT); - pinMode(dir2, OUTPUT); - - if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); - if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); - - // sanity check for the voltage limit configuration - if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; - - // Set the pwm frequency to the pins - // hardware specific function - depending on driver and mcu - _configure2PWM(pwm_frequency, pwm1, pwm2); - return 0; -} - - -// Set voltage to the pwm pin -void StepperDriver2PWM2Pin::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1(0.0),duty_cycle2(0.0); - // limit the voltage in driver - Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); - Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); - // hardware specific writing - duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); - // set direction - digitalWrite(dir2, Ualpha > 0 ? HIGH : LOW); - digitalWrite(dir1, Ubeta > 0 ? HIGH : LOW); - - _delay(1); - // Serial.print(Ualpha); - // Serial.print("\t"); - // Serial.print(Ubeta); - // Serial.print("\t"); - // Serial.print(duty_cycle1); - // Serial.print("\t"); - // Serial.println(duty_cycle2); - - // write to hardware - _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); -} \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM2Pin.h b/src/drivers/StepperDriver2PWM2Pin.h deleted file mode 100644 index 8b6e17aa..00000000 --- a/src/drivers/StepperDriver2PWM2Pin.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef STEPPER_DRIVER_2PWM_2PIN_h -#define STEPPER_DRIVER_2PWM_2PIN_h - -#include "../common/base_classes/StepperDriver.h" -#include "../common/foc_utils.h" -#include "../common/time_utils.h" -#include "../common/defaults.h" -#include "hardware_api.h" - -/** - 2 pwm stepper driver class -*/ -class StepperDriver2PWM2Pin: public StepperDriver -{ - public: - /** - StepperMotor class constructor - @param ph1PWM PWM1 phase pwm pin - @param ph1Dir DIR1 phase dir pin - @param ph2PWM PWM2 phase pwm pin - @param ph2Dir DIR2 phase dir pin - @param en1 enable pin phase 1 (optional input) - @param en2 enable pin phase 2 (optional input) - */ - StepperDriver2PWM2Pin(int ph1PWM, int ph1Dir,int ph2PWM, int ph2Dir, int en1 = NOT_SET, int en2 = NOT_SET); - - /** Motor hardware init function */ - int init() override; - /** Motor disable function */ - void disable() override; - /** Motor enable function */ - void enable() override; - - // hardware variables - int pwm1; //!< phase 1 pwm pin number - int dir1; //!< phase 1 dir pin number - int pwm2; //!< phase 2 pwm pin number - int dir2; //!< phase 2 dir pin number - int enable_pin1; //!< enable pin number phase 1 - int enable_pin2; //!< enable pin number phase 2 - - /** - * Set phase voltages to the harware - * - * @param Ua phase A voltage - * @param Ub phase B voltage - */ - void setPwm(float Ua, float Ub) override; - - private: - -}; - - -#endif From cd2070447d85e238278922d6ba755c5c9ea9e741 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 11 Feb 2021 17:58:20 +0100 Subject: [PATCH 080/749] added include --- src/SimpleFOC.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 6edc6784..27b9ab84 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -107,7 +107,6 @@ void loop() { #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" -#include "drivers/StepperDriver2PWM2Pin.h" #include "current_sense/InlineCurrentSense.h" #endif From a525cacb4d9b1d5c1e60fb392e86955d5b7cb227 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Feb 2021 11:28:40 +0100 Subject: [PATCH 081/749] restructured sensors and improved colibration --- src/BLDCMotor.cpp | 210 +++++++++++++---------- src/StepperMotor.cpp | 25 +-- src/common/base_classes/FOCMotor.cpp | 11 +- src/common/base_classes/FOCMotor.h | 22 ++- src/common/base_classes/Sensor.h | 4 - src/current_sense/InlineCurrentSense.cpp | 25 ++- src/sensors/Encoder.cpp | 4 +- src/sensors/HallSensor.cpp | 4 +- src/sensors/MagneticSensorAnalog.cpp | 2 +- src/sensors/MagneticSensorI2C.cpp | 4 +- src/sensors/MagneticSensorSPI.cpp | 4 +- 11 files changed, 187 insertions(+), 128 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 3eefe138..6ac04034 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -9,7 +9,7 @@ BLDCMotor::BLDCMotor(int pp, float _R) // save pole pairs number pole_pairs = pp; // save phase resistance number - phase_resistance = _R == NOT_SET ? 3.0/2.0 : _R; + phase_resistance = _R; // torque control type is voltage by default torque_controller = TorqueControlType::voltage; } @@ -28,7 +28,7 @@ void BLDCMotor::init() { // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && phase_resistance != NOT_SET ) { - float new_voltage_limit = current_limit / (phase_resistance*1.5); // v_lim = current_lim / (3/2 phase resistance) - worst case + float new_voltage_limit = current_limit * (phase_resistance*1.5); // v_lim = current_lim / (3/2 phase resistance) - worst case // use it if it is less then voltage_limit set by the user voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; } @@ -82,27 +82,30 @@ void BLDCMotor::enable() FOC functions */ // FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) { int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! if(zero_electric_offset != NOT_SET){ - if(monitor_port) monitor_port->println(F("MOT: Skip align.")); // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW - sensor->natural_direction = sensor_direction; - }else{ - // sensor and motor alignment - _delay(500); - if(sensor) exit_flag = alignSensor(); - else if(monitor_port) monitor_port->println(F("MOT: No sensor attached.")); - - _delay(500); - if(current_sense) exit_flag = alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense attached.")); + natural_direction = sensor_direction; } + // sensor and motor alignment - can be skipped + // by setting motor.natural_direction and motor.zero_electric_angle + _delay(500); + if(sensor) exit_flag = alignSensor(); + else if(monitor_port) monitor_port->println(F("MOT: No sensor attached.")); + + // alling the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + _delay(500); + if(current_sense) exit_flag = alignCurrentSense(); + else if(monitor_port) monitor_port->println(F("MOT: No current sense attached.")); + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); }else{ @@ -120,17 +123,22 @@ int BLDCMotor::alignCurrentSense() { if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); // make sure everything is stopped setPhaseVoltage(0, 0, 0); - - if(!current_sense->driverSync(driver, voltage_sensor_align)){ // align current sense and the driver + // align current sense and the driver + exit_flag = current_sense->driverSync(driver, voltage_sensor_align); + if(!exit_flag){ // error in current sense - phase either not measured or bad connection if(monitor_port) monitor_port->println(F("MOT: Current sense align error!")); exit_flag = 0; + }else{ + // output the alignment status flag + if(monitor_port) monitor_port->print(F("MOT: Success: ")); + if(monitor_port) monitor_port->println(exit_flag); } // make sure the motor is disabled setPhaseVoltage(0, 0, 0); _delay(200); - return exit_flag; + return exit_flag > 0; } // Encoder alignment to electrical 0 angle @@ -138,52 +146,55 @@ int BLDCMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - // check if sensor needs zero search - if(sensor->needsSearch()) absoluteZeroSearch(); - _delay(500); - - // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees - float start_angle = shaftAngle(); - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - // move one electrical revolution forward - _delay(500); - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // take and angle in the middle - float mid_angle = shaftAngle(); - // move one electrical revolution forward - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // determine the direction the sensor moved - if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); - sensor->natural_direction = Direction::CCW; - } else if (mid_angle == start_angle) { - if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); - exit_flag = 0; // failed calibration - } else{ - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); - } + // if unknown natural direction + if(natural_direction == NOT_SET){ + // check if sensor needs zero search + if(sensor->needsSearch()) absoluteZeroSearch(); + _delay(500); - // let the motor stabilize for 1 sec - _delay(1000); - // set sensor to zero - zero_electric_angle = _normalizeAngle(_electricalAngle(shaftAngle(), pole_pairs)); - _delay(500); - setPhaseVoltage(0, 0, 0); - _delay(200); - + // find natural direction + float start_angle = sensor->getAngle(); + // move one electrical revolution forward + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(2); + } + // take and angle in the middle + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0 ; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(2); + } + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); + natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); + exit_flag = 0; // failed calibration + } else{ + if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); + natural_direction = Direction::CW; + } + }else if(monitor_port) monitor_port->println(F("MOT: Skip direction calib.")); + + // zero electric angle not known + if(zero_electric_angle == NOT_SET){ + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + zero_electric_angle = _normalizeAngle(_electricalAngle(natural_direction*sensor->getAngle(), pole_pairs)); + _delay(20); + }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); return exit_flag; } - // Encoder alignment the absolute zero angle // - to the index void BLDCMotor::absoluteZeroSearch() { @@ -212,10 +223,13 @@ void BLDCMotor::absoluteZeroSearch() { void BLDCMotor::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(); - electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs) - zero_electric_angle); + // electrical angle - need shaftAngle to be called first + electrical_angle = electricalAngle(); switch (torque_controller) { case TorqueControlType::voltage: @@ -279,45 +293,46 @@ void BLDCMotor::move(float new_target) { // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - voltage.q = current_sp*1.5*phase_resistance; + // use voltage if phase-resistance not provided + if(phase_resistance == NOT_SET) voltage.q = current_sp; + else voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; - }else{ - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control } break; case MotionControlType::velocity: // velocity set point shaft_velocity_sp = target; // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control if(torque_controller == TorqueControlType::voltage){ - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - voltage.q = current_sp*1.5*phase_resistance; + // use voltage if phase-resistance not provided + if(phase_resistance == NOT_SET) voltage.q = current_sp; + else voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; - }else{ - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control } break; case MotionControlType::velocity_openloop: // velocity control in open loop - // loopFOC should not be called shaft_velocity_sp = target; velocityOpenloop(shaft_velocity_sp); + voltage.q = voltage_limit; voltage.d = 0; break; case MotionControlType::angle_openloop: // angle control in open loop - // loopFOC should not be called shaft_angle_sp = target; angleOpenloop(shaft_angle_sp); + voltage.q = voltage_limit; voltage.d = 0; break; } } - // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Space Vector PWM and Sine PWM algorithms // @@ -326,7 +341,6 @@ void BLDCMotor::move(float new_target) { // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - const bool centered = true; float center; int sector; float _ca,_sa; @@ -341,9 +355,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // static int trap_120_state = 0; sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either - // centered == true > driver.volage_limit/2 - // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = centered ? (driver->voltage_limit)/2 : Uq; + // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + center = modulation_centered ? (driver->voltage_limit)/2 : Uq; if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){ Ua= center; @@ -372,9 +386,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // static int trap_150_state = 0; sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either - // centered == true > driver.volage_limit/2 - // centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = centered ? (driver->voltage_limit)/2 : Uq; + // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + center = modulation_centered ? (driver->voltage_limit)/2 : Uq; if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){ Ua= center; @@ -408,14 +422,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; - // center = centered ? (driver->voltage_limit)/2 : Uq; + // center = modulation_centered ? (driver->voltage_limit)/2 : Uq; center = driver->voltage_limit/2; // Clarke transform Ua = Ualpha + center; Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + center; Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + center; - if (!centered) { + if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); Ua -= Umin; Ub -= Umin; @@ -438,12 +452,20 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // equivalent to 3) is // angle_el = angle_el + atan2(Uq,Ud) - // _sqrt is an approx of sqrt (3-4% error) - float Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); - + float Uout; + // a bit of optitmisation + if(Ud){ // only if Ud and Uq set + // _sqrt is an approx of sqrt (3-4% error) + Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); + }else{// only Uq available - no need for atan2 and sqrt + Uout = Uq / driver->voltage_limit; + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + _PI_2); + } // find the sector we are in currently sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles @@ -451,8 +473,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage - if (centered) { - T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + if (modulation_centered) { + T0 = 1 - T1 - T2; //modulation_centered around driver->voltage_limit/2 } // calculate the duty cycles(times) @@ -519,7 +541,9 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ float Ts = (now_us - open_loop_timestamp) * 1e-6; // calculate the necessary angle to achieve target velocity - shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); + shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); + // for display purposes + shaft_velocity = target_velocity; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -539,11 +563,13 @@ void BLDCMotor::angleOpenloop(float target_angle){ // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) - if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; - else + shaft_velocity = velocity_limit; + }else{ shaft_angle = target_angle; - + shaft_velocity = 0; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 256acc9a..627c379d 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -76,7 +76,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_directi // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW - sensor->natural_direction = sensor_direction; + natural_direction = sensor_direction; }else{ // sensor and motor alignment _delay(500); @@ -118,7 +118,7 @@ int StepperMotor::alignSensor() { // determine the direction the sensor moved if (mid_angle < start_angle) { if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); - sensor->natural_direction = Direction::CCW; + natural_direction = Direction::CCW; } else if (mid_angle == start_angle) { if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); } else{ @@ -253,6 +253,8 @@ void StepperMotor::velocityOpenloop(float target_velocity){ // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); + // for display purposes + shaft_velocity = target_velocity; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); @@ -264,22 +266,23 @@ void StepperMotor::velocityOpenloop(float target_velocity){ // Function (iterative) generating open loop movement towards the target angle // - target_angle - rad // it uses voltage_limit and velocity_limit variables -void StepperMotor::angleOpenloop(float target_angle){ +void BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; - + // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) - if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; - else + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + shaft_velocity = velocity_limit; + }else{ shaft_angle = target_angle; - + shaft_velocity = 0; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call - open_loop_timestamp = now_us; -} \ No newline at end of file + open_loop_timestamp = now_us; \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 2474a173..3137cfe8 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -17,9 +17,6 @@ FOCMotor::FOCMotor() // sensor and motor align voltage voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; - // electric angle of comthe zero angle - zero_electric_angle = 0; - // default modulation is SinePWM foc_modulation = FOCModulationType::SinePWM; @@ -59,13 +56,17 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) { float FOCMotor::shaftAngle() { // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_angle; - return sensor->getAngle(); + return natural_direction*sensor->getAngle() - sensor_offset; } // shaft velocity calculation float FOCMotor::shaftVelocity() { // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_velocity; - return LPF_velocity(sensor->getVelocity()); + return natural_direction*LPF_velocity(sensor->getVelocity()); +} + +float FOCMotor::electricalAngle(){ + return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); } /** diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index d6868013..aaeddd87 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -112,6 +112,12 @@ class FOCMotor */ float shaftVelocity(); + + /** + * Electrical angle calculation + */ + float electricalAngle(); + // state variables float target; //!< current target value - depends of the controller float shaft_angle;//!< current motor angle @@ -130,7 +136,6 @@ class FOCMotor // motor physical parameters float phase_resistance; //!< motor phase resistance int pole_pairs;//!< motor pole pairs number - float zero_electric_angle;//!< absolute zero electric angle - if available // limiting variables float voltage_limit; //!< Voltage limitting variable - global limit @@ -138,12 +143,18 @@ class FOCMotor float velocity_limit; //!< Velocity limitting variable - global limit // motor status vairables - int enabled = 0; + int enabled = 0;//!< enabled or disabled motor flag + + // pwm modulation related variables + FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm + int modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2 or (0) pulled to 0 + // configuration structures TorqueControlType torque_controller; //!< parameter determining the torque control type MotionControlType controller; //!< parameter determining the control loop to be used - FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm + + // controllers and low pass filters PIDController PID_current_q{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the q current PID config PIDController PID_current_d{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the d current PID config LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!< parameter determining the current Low pass filter configuration @@ -152,6 +163,11 @@ class FOCMotor PIDController P_angle{DEF_P_ANGLE_P,0,0,1e10,DEF_VEL_LIM}; //!< parameter determining the position PID configuration LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!< parameter determining the velocity Low pass filter configuration + // sensor related variabels + float sensor_offset; //!< user defined sensor zero offset + float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available + int natural_direction = NOT_SET; //!< if natural_direction == Direction::CCW then direction will be flipped to CW + /** * Function providing BLDCMotor class with the * Serial interface and enabling monitoring mode diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 51b91e99..b710a512 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -25,10 +25,6 @@ enum Pullup{ class Sensor{ public: - // if natural_direction == Direction::CCW then direction will be flipped to CW - int natural_direction = Direction::CW; - float zero_offset = 0; //!< user defined zero offset - /** get current angle (rad) */ virtual float getAngle()=0; /** get current angular velocity (rad/s)*/ diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 6f5b5d51..cd00d39e 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -51,12 +51,18 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ // Function synchronizing and aligning the current sense with motor driver // if all pins are connected well none of this is really necessary! - can be avoided -// returns 0 for failure and 1 for success +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ + int exit_flag = 1; // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); - _delay(1000); + _delay(200); PhaseCurrent_s c = getPhaseCurrents(); // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { @@ -78,12 +84,14 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ pinA = pinB; pinB = tmp_pinA; gain_adjust_a = _sign(c.b); + exit_flag = 2; // signal that pins have been switched }else if(pinC != NOT_SET && ac_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; pinA = pinC; pinC= tmp_pinA; gain_adjust_a = _sign(c.c); + exit_flag = 2;// signal that pins have been switched }else{ // error in current sense - phase either not measured or bad connection return 0; @@ -91,7 +99,7 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ // set phase B active and phases A and C down driver->setPwm(0, voltage, 0); - _delay(1000); + _delay(200); c = getPhaseCurrents(); // read the current 50 times for (int i = 0; i < 100; i++) { @@ -112,12 +120,14 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ pinB = pinA; pinA = tmp_pinB; gain_adjust_b = _sign(c.a); + exit_flag = 2; // signal that pins have been switched }else if(pinC != NOT_SET && bc_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; pinB = pinC; pinC = tmp_pinB; gain_adjust_b = _sign(c.c); + exit_flag = 2; // signal that pins have been switched }else{ // error in current sense - phase either not measured or bad connection return 0; @@ -138,6 +148,13 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ gain_adjust_c = _sign(c.c); } - return 1; + if(gain_adjust_a < 0 || gain_adjust_b < 0 || gain_adjust_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + return exit_flag; } diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 89daaba9..76e17429 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -102,7 +102,7 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getAngle(){ - return natural_direction * _2PI * (pulse_counter) / ((float)cpr) - zero_offset; + return _2PI * (pulse_counter) / ((float)cpr); } /* Shaft velocity calculation @@ -140,7 +140,7 @@ float Encoder::getVelocity(){ // save velocity calculation variables prev_Th = Th; prev_pulse_counter = pulse_counter; - return natural_direction * (velocity); + return velocity; } // getter for index pin diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2e3ecefe..e39fd514 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,7 +96,7 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { Shaft angle calculation */ float HallSensor::getAngle() { - return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI - zero_offset; + return ((electric_rotations * 6 + electric_sector) / cpr) * _2PI ; } /* @@ -107,7 +107,7 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return natural_direction * direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); } } diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index a1fd207f..0216392a 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -43,7 +43,7 @@ float MagneticSensorAnalog::getAngle(){ // if overflow happened track it as full rotation if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - float angle = natural_direction * (full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI) - zero_offset; + float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; // calculate velocity here long now = _micros(); diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index b07fb4b2..531a06f9 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -90,8 +90,8 @@ float MagneticSensorI2C::getAngle(){ // in order to know if overflow happened angle_data_prev = angle_data; // return the full angle - // (number of full rotations)*2PI + current sensor angle - zero_offset - return (natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) - zero_offset); + // (number of full rotations)*2PI + current sensor angle + return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; } // Shaft velocity calculation diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 8a842098..56833b7f 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -106,8 +106,8 @@ float MagneticSensorSPI::getAngle(){ angle_data_prev = angle_data; // return the full angle - // (number of full rotations)*2PI + current sensor angle - zero_offset - return (natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) - zero_offset); + // (number of full rotations)*2PI + current sensor angle + return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; } // Shaft velocity calculation From 8b99aee840f9c3dd98dbafadd1b9e66e0c72e7cc Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Feb 2021 11:38:23 +0100 Subject: [PATCH 082/749] FIX stepper typo + added list of features --- README.md | 18 +++++++++++------- src/StepperMotor.cpp | 5 +++-- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 31729d90..5443d57f 100644 --- a/README.md +++ b/README.md @@ -15,20 +15,24 @@ Therefore this is an attempt to: ##### NEXT RELEASE 📢: SimpleFOClibrary v2.1 > #### Implemented features in dev branch -> - 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 ) -> - included F macro for shrinking string memory usage - moved to programming memory -> - **Initial current sensing support** +> - **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 -> - SVPWM full implementation d+q axis > > BEWARE 📢 slight API changes included > - `ControlType` renamed into `MotionControlType` diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 627c379d..ef9b827d 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -266,7 +266,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){ // Function (iterative) generating open loop movement towards the target angle // - target_angle - rad // it uses voltage_limit and velocity_limit variables -void BLDCMotor::angleOpenloop(float target_angle){ +void StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call @@ -285,4 +285,5 @@ void BLDCMotor::angleOpenloop(float target_angle){ setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call - open_loop_timestamp = now_us; \ No newline at end of file + open_loop_timestamp = now_us; +} \ No newline at end of file From 9787eb0f0cb65f0f624c8e75810e29e581fa5983 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Feb 2021 11:43:34 +0100 Subject: [PATCH 083/749] FIX example typo --- .../find_sensor_offset_and_direction.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index fe8ea8f2..9cdc5656 100644 --- a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -54,7 +54,7 @@ void setup() { Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); - Serial.println(sensor.natural_direction == 1 ? "Direction::CW" : "Direction::CCW"); + Serial.println(motor.natural_direction == 1 ? "Direction::CW" : "Direction::CCW"); Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)"); _delay(1000); From bd246dc58fa2ff95cb73cd346a9e995ffefd0c4a Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Feb 2021 12:26:20 +0100 Subject: [PATCH 084/749] fail initFOC if any of the sensors not aligned --- src/BLDCMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 6ac04034..0dee792e 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -96,14 +96,14 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) // sensor and motor alignment - can be skipped // by setting motor.natural_direction and motor.zero_electric_angle _delay(500); - if(sensor) exit_flag = alignSensor(); + if(sensor) exit_flag *= alignSensor(); else if(monitor_port) monitor_port->println(F("MOT: No sensor attached.")); // alling the current sensor - can be skipped // checks if driver phases are the same as current sense phases // and checks the direction of measuremnt. _delay(500); - if(current_sense) exit_flag = alignCurrentSense(); + if(current_sense) exit_flag *= alignCurrentSense(); else if(monitor_port) monitor_port->println(F("MOT: No current sense attached.")); if(exit_flag){ From 35a697fbb9476701f2d546ca34a939d7d3fb082e Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Feb 2021 15:25:59 +0100 Subject: [PATCH 085/749] Added zero electric angle in output --- .../open_loop_position_example.ino | 2 +- .../open_loop_velocity_example.ino | 4 +- .../current_control/current_control.ino | 125 ++++++++++++++++++ .../inline_current_sense_test.ino | 36 +++++ src/BLDCMotor.cpp | 6 +- 5 files changed, 169 insertions(+), 4 deletions(-) create mode 100644 examples/motion_control/torque_control/encoder/current_control/current_control.ino create mode 100644 examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index edf239bf..7e35441f 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -23,7 +23,7 @@ void setup() { // limiting motor movements motor.voltage_limit = 3; // [V] - motor.velocity_limit = 20; // [rad/s] + motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::angle_openloop; diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 1aa0a93d..885884f0 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -23,8 +23,8 @@ void setup() { // limiting motor movements motor.voltage_limit = 3; // [V] - motor.velocity_limit = 20; // [rad/s] - + motor.velocity_limit = 5; // [rad/s] cca 50rpm + // open loop control config motor.controller = MotionControlType::velocity_openloop; diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino new file mode 100644 index 00000000..71e86b4b --- /dev/null +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -0,0 +1,125 @@ +/** + * + * Torque control example using current control loop. + * + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// current sensor +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set torque mode: + // TorqueControlType::current + // TorqueControlType::voltage + // TorqueControlType::foc_current + motor.torque_controller = TorqueControlType::foc_current; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // foc currnet control parameters (Arduino UNO/Mega) + motor.PID_current_q.P = 5; + motor.PID_current_q.I= 300; + motor.PID_current_d.P= 5; + motor.PID_current_d.I = 300; + motor.LPF_current_q.Tf = 0.01; + motor.LPF_current_d.Tf = 0.01; + // foc currnet control parameters (stm/esp/due/teensy) + // motor.PID_current_q.P = 5; + // motor.PID_current_q.I= 1000; + // motor.PID_current_d.P= 5; + // motor.PID_current_d.I = 1000; + // motor.LPF_current_q.Tf = 0.002; // 1ms default + // motor.LPF_current_d.Tf = 0.002; // 1ms default + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + +// target voltage to be set to the motor +float target_current = 0; + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or torque (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_current); + + // communicate with the user + serialReceiveUserCommand(); +} + + +// utility function enabling serial communication with the user to set the target values +// this function can be implemented in serialEvent function as well +void serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + + // change the motor target + target_current = received_chars.toFloat(); + Serial.print("Target current: "); + Serial.println(target_current); + + // reset the command buffer + received_chars = ""; + } + } +} \ No newline at end of file diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino new file mode 100644 index 00000000..45a20a56 --- /dev/null +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -0,0 +1,36 @@ +/** + * Testing example code for the Inline current sensing class +*/ +#include + +// current sensor +// shunt resistor value +// gain value +// pins phase A,B, (C optional) +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); + + +void setup() { + // initialise the current sensing + current_sense.init(); + + // for SimpleFOCShield v2.01/v2.0.2 + current_sense.gain_adjust_b = -1; + + Serial.begin(115200); + Serial.println("Current sense ready."); +} + +void loop() { + + PhaseCurrent_s currents = current_sense.getPhaseCurrents(); + float current_magnitude = current_sense.getCurrents(); + + Serial.print(currents.a*1000); // milli Amps + Serial.print("\t"); + Serial.print(currents.b*1000); // milli Amps + Serial.print("\t"); + Serial.print(currents.c*1000); // milli Amps + Serial.print("\t"); + Serial.println(current_magnitude*1000); // milli Amps +} diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 0dee792e..f5f7d4ff 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -99,7 +99,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) if(sensor) exit_flag *= alignSensor(); else if(monitor_port) monitor_port->println(F("MOT: No sensor attached.")); - // alling the current sensor - can be skipped + // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases // and checks the direction of measuremnt. _delay(500); @@ -191,6 +191,10 @@ int BLDCMotor::alignSensor() { _delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(natural_direction*sensor->getAngle(), pole_pairs)); _delay(20); + if(monitor_port){ + monitor_port->print(F("MOT: Zero elec. angle: ")); + monitor_port->println(zero_electric_angle); + } }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); return exit_flag; } From d4e160d69692b5effabf87a1a765b26c041a60c2 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Feb 2021 15:44:08 +0100 Subject: [PATCH 086/749] typo example current sense --- .../inline_current_sense_test/inline_current_sense_test.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 45a20a56..20c62323 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -24,7 +24,7 @@ void setup() { void loop() { PhaseCurrent_s currents = current_sense.getPhaseCurrents(); - float current_magnitude = current_sense.getCurrents(); + float current_magnitude = current_sense.getCurrent(); Serial.print(currents.a*1000); // milli Amps Serial.print("\t"); From 9cd9e2840d7c3d2705227fa03b08057c59d93bae Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Feb 2021 16:08:52 +0100 Subject: [PATCH 087/749] Bugfix unhandled first time execution openloop --- src/BLDCMotor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index f5f7d4ff..c6edb4da 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -209,6 +209,7 @@ void BLDCMotor::absoluteZeroSearch() { velocity_limit = velocity_index_search; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ + if(monitor_port) monitor_port->println(shaft_angle); angleOpenloop(1.5*_2PI); } // disable motor @@ -543,6 +544,8 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; + // quick fix for strange cases (micros overflow + timestamp not defined) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -564,6 +567,8 @@ void BLDCMotor::angleOpenloop(float target_angle){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; + // quick fix for strange cases (micros overflow + timestamp not defined) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) From bfec5313edbf799e63498d6d3457eefc71433aa5 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Feb 2021 16:09:22 +0100 Subject: [PATCH 088/749] Bugfix unhandled first time execution openloop stepper --- src/StepperMotor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index ef9b827d..cbc7f8cc 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -250,6 +250,8 @@ void StepperMotor::velocityOpenloop(float target_velocity){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; + // quick fix for strange cases (micros overflow + timestamp not defined) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -271,6 +273,8 @@ void StepperMotor::angleOpenloop(float target_angle){ unsigned long now_us = _micros(); // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; + // quick fix for strange cases (micros overflow + timestamp not defined) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) From 626ab8f9e64cd5495a2e1f0663637c1f08dda28c Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Feb 2021 16:28:30 +0100 Subject: [PATCH 089/749] forgot to remove serial print --- src/BLDCMotor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c6edb4da..69e8a225 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -209,7 +209,6 @@ void BLDCMotor::absoluteZeroSearch() { velocity_limit = velocity_index_search; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - if(monitor_port) monitor_port->println(shaft_angle); angleOpenloop(1.5*_2PI); } // disable motor From 86fc16171076b3f28b4bb97b004628f1b3634bd9 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Feb 2021 19:37:18 +0100 Subject: [PATCH 090/749] added pole pair check, restructured curent sense + a lot of testing --- README.md | 5 ++ .../{ => osc_esp32_3pwm}/layout1.touchosc | Bin .../osc_esp32_3pwm.ino} | 0 .../find_sensor_offset_and_direction.ino | 2 +- keywords.txt | 12 ++- src/BLDCMotor.cpp | 70 ++++++++++-------- src/BLDCMotor.h | 2 +- src/StepperMotor.cpp | 10 +-- src/common/base_classes/BLDCDriver.h | 10 +-- src/common/base_classes/CurrentSense.cpp | 2 +- src/common/base_classes/CurrentSense.h | 15 +++- src/common/base_classes/FOCMotor.cpp | 5 +- src/common/base_classes/FOCMotor.h | 3 +- src/common/base_classes/Sensor.h | 1 + src/common/base_classes/StepperDriver.h | 8 +- src/current_sense/InlineCurrentSense.cpp | 38 ++++++---- src/current_sense/InlineCurrentSense.h | 13 ++-- src/sensors/HallSensor.h | 2 +- 18 files changed, 119 insertions(+), 79 deletions(-) rename examples/osc_control_examples/{ => osc_esp32_3pwm}/layout1.touchosc (100%) rename examples/osc_control_examples/{simplefoc_osc_esp32_3pwm.ino => osc_esp32_3pwm/osc_esp32_3pwm.ino} (100%) diff --git a/README.md b/README.md index 5443d57f..258358f1 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,11 @@ Therefore this is an attempt to: > - 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`) > > BEWARE 📢 slight API changes included > - `ControlType` renamed into `MotionControlType` diff --git a/examples/osc_control_examples/layout1.touchosc b/examples/osc_control_examples/osc_esp32_3pwm/layout1.touchosc similarity index 100% rename from examples/osc_control_examples/layout1.touchosc rename to examples/osc_control_examples/osc_esp32_3pwm/layout1.touchosc diff --git a/examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino similarity index 100% rename from examples/osc_control_examples/simplefoc_osc_esp32_3pwm.ino rename to examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino diff --git a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 9cdc5656..a54fcea4 100644 --- a/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -54,7 +54,7 @@ void setup() { Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); - Serial.println(motor.natural_direction == 1 ? "Direction::CW" : "Direction::CCW"); + Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW"); Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)"); _delay(1000); diff --git a/keywords.txt b/keywords.txt index 17a72a99..92815269 100644 --- a/keywords.txt +++ b/keywords.txt @@ -77,6 +77,9 @@ getPhaseCurrents KEYWORD2 getFOCCurrents KEYWORD2 getCurrent KEYWORD2 setPwm KEYWORD2 +driverAlign KEYWORD2 +driverSync KEYWORD2 + voltage_q KEYWORD2 voltage_d KEYWORD2 @@ -95,9 +98,12 @@ foc_modulation KEYWORD2 target KEYWORD2 pwm_frequency KEYWORD2 dead_zone KEYWORD2 -gain_adjust_a KEYWORD2 -gain_adjust_b KEYWORD2 -gain_adjust_c KEYWORD2 +gain_a KEYWORD2 +gain_b KEYWORD2 +gain_c KEYWORD2 +skip_align KEYWORD2 +sensor_direction KEYWORD2 +sensor_offset KEYWORD2 voltage KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 69e8a225..113ef542 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -24,7 +24,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Initialise variables.")); + if(monitor_port) monitor_port->println(F("MOT: Init")); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && phase_resistance != NOT_SET ) { @@ -82,7 +82,7 @@ void BLDCMotor::enable() FOC functions */ // FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) { +int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! @@ -90,26 +90,26 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW - natural_direction = sensor_direction; + sensor_direction = _sensor_direction; } // sensor and motor alignment - can be skipped - // by setting motor.natural_direction and motor.zero_electric_angle + // 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 attached.")); + 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 // and checks the direction of measuremnt. _delay(500); if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense attached.")); + else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); + if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ - if(monitor_port) monitor_port->println(F("MOT: Calibration failed.")); + if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } @@ -121,23 +121,19 @@ int BLDCMotor::alignCurrentSense() { int exit_flag = 1; // success if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); - // make sure everything is stopped - setPhaseVoltage(0, 0, 0); + // align current sense and the driver - exit_flag = current_sense->driverSync(driver, voltage_sensor_align); + exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); if(!exit_flag){ // error in current sense - phase either not measured or bad connection - if(monitor_port) monitor_port->println(F("MOT: Current sense align error!")); + if(monitor_port) monitor_port->println(F("MOT: Align error!")); exit_flag = 0; }else{ // output the alignment status flag if(monitor_port) monitor_port->print(F("MOT: Success: ")); if(monitor_port) monitor_port->println(exit_flag); } - // make sure the motor is disabled - setPhaseVoltage(0, 0, 0); - _delay(200); - + return exit_flag > 0; } @@ -147,13 +143,13 @@ int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); // if unknown natural direction - if(natural_direction == NOT_SET){ + if(sensor_direction == NOT_SET){ // check if sensor needs zero search - if(sensor->needsSearch()) absoluteZeroSearch(); - _delay(500); + if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); + // stop init if not found index + if(!exit_flag) return exit_flag; // find natural direction - float start_angle = sensor->getAngle(); // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0; @@ -168,20 +164,29 @@ int BLDCMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } + float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved - if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); - natural_direction = Direction::CCW; - } else if (mid_angle == start_angle) { - if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); - exit_flag = 0; // failed calibration + if (mid_angle == end_angle) { + if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + return 0; // failed calibration + } else if (mid_angle < end_angle) { + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); - natural_direction = Direction::CW; + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + sensor_direction = Direction::CW; } - }else if(monitor_port) monitor_port->println(F("MOT: Skip direction calib.")); + // check pole pair number + if(monitor_port) monitor_port->print(F("MOT: PP check: ")); + float moved = fabs(mid_angle - end_angle); + if( fabs(moved*pole_pairs - _2PI) > 0.25 ) { // 0.25 is arbitrary number it can be lower or higher! + if(monitor_port) monitor_port->println(F("fail!")); + return 0; // failed calibration + }else if(monitor_port) monitor_port->println(F("OK!")); + + }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); // zero electric angle not known if(zero_electric_angle == NOT_SET){ @@ -189,7 +194,7 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(natural_direction*sensor->getAngle(), pole_pairs)); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -201,9 +206,9 @@ int BLDCMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index -void BLDCMotor::absoluteZeroSearch() { +int BLDCMotor::absoluteZeroSearch() { - if(monitor_port) monitor_port->println(F("MOT: Absolute zero search...")); + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit = velocity_limit; velocity_limit = velocity_index_search; @@ -220,6 +225,7 @@ void BLDCMotor::absoluteZeroSearch() { if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); else monitor_port->println(F("MOT: Success!")); } + return !sensor->needsSearch(); } // Iterative function looping FOC algorithm, setting Uq on the Motor diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index af1eb2b6..761c1ebb 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -86,7 +86,7 @@ class BLDCMotor: public FOCMotor /** Current sense and motor phase alignment */ int alignCurrentSense(); /** Motor and sensor alignment to the sensors absolute 0 angle */ - void absoluteZeroSearch(); + int absoluteZeroSearch(); // Open loop motion control diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index cbc7f8cc..91d91763 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -68,7 +68,7 @@ void StepperMotor::enable() FOC functions */ // FOC initialization function -int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { +int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! @@ -76,7 +76,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_directi // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW - natural_direction = sensor_direction; + sensor_direction = _sensor_direction; }else{ // sensor and motor alignment _delay(500); @@ -117,12 +117,12 @@ int StepperMotor::alignSensor() { } // determine the direction the sensor moved if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CCW")); - natural_direction = Direction::CCW; + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + sensor_direction = Direction::CCW; } else if (mid_angle == start_angle) { if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); } else{ - if(monitor_port) monitor_port->println(F("MOT: natural_direction==CW")); + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); } // let the motor stabilize for 1 sec diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index eeec2e8e..ea96d3fa 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,11 +5,11 @@ class BLDCDriver{ public: /** Initialise hardware */ - virtual int init(); + virtual int init() = 0; /** Enable hardware */ - virtual void enable(); + virtual void enable() = 0; /** Disable hardware */ - virtual void disable(); + virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz float voltage_power_supply; //!< power supply voltage @@ -27,7 +27,7 @@ class BLDCDriver{ * @param Ub - phase B voltage * @param Uc - phase C voltage */ - virtual void setPwm(float Ua, float Ub, float Uc); + virtual void setPwm(float Ua, float Ub, float Uc) = 0; /** * Set phase voltages to the harware @@ -36,7 +36,7 @@ class BLDCDriver{ * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) */ - virtual void setPhaseState(int sa, int sb, int sc); + virtual void setPhaseState(int sa, int sb, int sc) = 0; }; #endif \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index b21a72a4..57d48025 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -45,7 +45,7 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ 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_alpha = 0.6666667*(current.a - (current.b - current.c)); i_beta = _2_SQRT3 *( current.b - current.c ); } diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index d894ad34..1b8a79af 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -18,11 +18,11 @@ class CurrentSense{ virtual void init() = 0; /** - * Function intended to implement all that is needed to sync and calibrate the current sensing with the driver. + * Function intended to implement all that is needed to sync and current sensing with the driver. * If no such thing is needed it can be left empty (return 1) * @returns - 0 - for failure & 1 - for success */ - virtual int driverSync(BLDCDriver *driver, float voltage) = 0; + virtual int driverSync(BLDCDriver *driver) = 0; /** * Function rading the phase currents a, b and c @@ -51,6 +51,17 @@ class CurrentSense{ */ DQCurrent_s getFOCCurrents(float angle_el); + // calibration variables + bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() + /** + * Function intended to verify if: + * - phase current are oriented properly + * - if their order is the same as driver phases + * + * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) + * @returns - 0 - for failure & positive number (with status) - for success + */ + virtual int driverAlign(BLDCDriver *driver, float voltage) = 0; }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 3137cfe8..0265f028 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -56,13 +56,13 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) { float FOCMotor::shaftAngle() { // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_angle; - return natural_direction*sensor->getAngle() - sensor_offset; + return sensor_direction*sensor->getAngle() - sensor_offset; } // shaft velocity calculation float FOCMotor::shaftVelocity() { // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_velocity; - return natural_direction*LPF_velocity(sensor->getVelocity()); + return sensor_direction*LPF_velocity(sensor->getVelocity()); } float FOCMotor::electricalAngle(){ @@ -77,6 +77,7 @@ void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); } + // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index aaeddd87..89cedf71 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -11,7 +11,6 @@ #include "../pid.h" #include "../lowpass_filter.h" - /** * Motiron control type */ @@ -166,7 +165,7 @@ class FOCMotor // sensor related variabels float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available - int natural_direction = NOT_SET; //!< if natural_direction == Direction::CCW then direction will be flipped to CW + int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW /** * Function providing BLDCMotor class with the diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index b710a512..c7771a05 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -10,6 +10,7 @@ enum Direction{ UNKNOWN = 0 //not yet known or invalid state }; + /** * Pullup configuration structure */ diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 7800a4e7..33f08843 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -5,11 +5,11 @@ class StepperDriver{ public: /** Initialise hardware */ - virtual int init(); + virtual int init() = 0; /** Enable hardware */ - virtual void enable(); + virtual void enable() = 0; /** Disable hardware */ - virtual void disable(); + virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz float voltage_power_supply; //!< power supply voltage @@ -21,7 +21,7 @@ class StepperDriver{ * @param Ua phase A voltage * @param Ub phase B voltage */ - virtual void setPwm(float Ua, float Ub); + virtual void setPwm(float Ua, float Ub) = 0; }; #endif \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index cd00d39e..cf06774a 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -13,6 +13,10 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ shunt_resistor = _shunt_resistor; amp_gain = _gain; volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; } // Inline sensor init function @@ -43,13 +47,18 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = gain_adjust_a*(_readADCVoltage(pinA) - offset_ia)*volts_to_amps_ratio;// amps - current.b = gain_adjust_b*(_readADCVoltage(pinB) - offset_ib)*volts_to_amps_ratio;// amps - current.c = (pinC == NOT_SET) ? 0 : gain_adjust_c*(_readADCVoltage(pinC) - offset_ic)*volts_to_amps_ratio; // amps + current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps + current.c = (pinC == NOT_SET) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps return current; } +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int InlineCurrentSense::driverSync(BLDCDriver *driver){ + return 1; +} -// Function synchronizing and aligning the current sense with motor driver +// Function aligning the current sense with motor driver // if all pins are connected well none of this is really necessary! - can be avoided // returns flag // 0 - fail @@ -57,9 +66,10 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ +int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ int exit_flag = 1; - + if(skip_align) return exit_flag; + // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); _delay(200); @@ -77,20 +87,20 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ float ab_ratio = fabs(c.a / c.b); float ac_ratio = c.c ? fabs(c.a / c.c) : 0; if( ab_ratio > 1.5 ){ // should be ~2 - gain_adjust_a = _sign(c.a); + gain_a *= _sign(c.a); }else if( ab_ratio < 0.7 ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; pinA = pinB; pinB = tmp_pinA; - gain_adjust_a = _sign(c.b); + gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched }else if(pinC != NOT_SET && ac_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; pinA = pinC; pinC= tmp_pinA; - gain_adjust_a = _sign(c.c); + gain_a *= _sign(c.c); exit_flag = 2;// signal that pins have been switched }else{ // error in current sense - phase either not measured or bad connection @@ -113,20 +123,20 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ float ba_ratio = fabs(c.b/c.a); float bc_ratio = c.c ? fabs(c.b / c.c) : 0; if( ba_ratio > 1.5 ){ // should be ~2 - gain_adjust_b = _sign(c.b); + gain_b *= _sign(c.b); }else if( ba_ratio < 0.7 ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; pinB = pinA; pinA = tmp_pinB; - gain_adjust_b = _sign(c.a); + gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched }else if(pinC != NOT_SET && bc_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; pinB = pinC; pinC = tmp_pinB; - gain_adjust_b = _sign(c.c); + gain_b *= _sign(c.c); exit_flag = 2; // signal that pins have been switched }else{ // error in current sense - phase either not measured or bad connection @@ -145,10 +155,10 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver, float voltage){ c.c = (c.c+c1.c)/50.0; } driver->setPwm(0, 0, 0); - gain_adjust_c = _sign(c.c); + gain_c *= _sign(c.c); } - if(gain_adjust_a < 0 || gain_adjust_b < 0 || gain_adjust_c < 0) exit_flag +=2; + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; // exit flag is either // 0 - fail // 1 - success and nothing changed diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 3ff645ae..64c658d1 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -23,14 +23,15 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions void init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver, float voltage) override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; - // ADC measuremnet gain adjustment for each phase - // if (shunt+amp combination) measures inverse current you can set it to -1 for example + // 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_adjust_a = 1; //!< phase A gain adjust - int gain_adjust_b = 1; //!< phase B gain adjust - int gain_adjust_c = 1; //!< phase C gain adjust + int gain_a; //!< phase A gain + int gain_b; //!< phase B gain + int gain_c; //!< phase C gain private: diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 167041ce..c836cc5d 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -58,7 +58,7 @@ class HallSensor: public Sensor{ /** get current angular velocity (rad/s) */ float getVelocity() override; - // whether last step was CW (+1) or CCW (-1). Note - this is a raw direction (i.e. doesn't include natural_direction reversal) + // whether last step was CW (+1) or CCW (-1). Direction direction; void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); From 5979a1be5a1ceec8d4d74616c4af1929dbe3bdfb Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Feb 2021 19:41:23 +0100 Subject: [PATCH 091/749] typo in example --- .../inline_current_sense_test/inline_current_sense_test.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 20c62323..5d8dd9b2 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -15,7 +15,7 @@ void setup() { current_sense.init(); // for SimpleFOCShield v2.01/v2.0.2 - current_sense.gain_adjust_b = -1; + current_sense.gain_b *= -1; Serial.begin(115200); Serial.println("Current sense ready."); From d5d9b98f038495df1b4a1514b4d0e3b4e19e7a38 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 16 Feb 2021 15:19:46 +0100 Subject: [PATCH 092/749] zero angle to keywords --- keywords.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/keywords.txt b/keywords.txt index 92815269..3bb6f48d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -104,6 +104,7 @@ gain_c KEYWORD2 skip_align KEYWORD2 sensor_direction KEYWORD2 sensor_offset KEYWORD2 +zero_electric_angle KEYWORD2 voltage KEYWORD2 From d7fb9dc45c414b443aab9d99608f019134db6712 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 16 Feb 2021 16:20:08 +0100 Subject: [PATCH 093/749] fix forgotten disable --- src/BLDCMotor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 113ef542..d16091c0 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -200,6 +200,9 @@ int BLDCMotor::alignSensor() { monitor_port->print(F("MOT: Zero elec. angle: ")); monitor_port->println(zero_electric_angle); } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); return exit_flag; } @@ -266,7 +269,6 @@ void BLDCMotor::loopFOC() { voltage.q = PID_current_q(current_sp - current.q); voltage.d = PID_current_d(-current.d); break; - default: // no torque control selected if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); From 232dcce52ddacbccb986421f5851e75f66f7c540 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 17 Feb 2021 14:36:31 +0100 Subject: [PATCH 094/749] FEAT new communication protocol --- keywords.txt | 2 - src/BLDCMotor.cpp | 14 +- src/BLDCMotor.h | 1 + src/SimpleFOC.h | 1 + src/StepperMotor.cpp | 4 +- src/common/base_classes/BLDCDriver.h | 2 + src/common/base_classes/CommunicationNode.h | 15 + src/common/base_classes/FOCMotor.cpp | 291 ++++++++---------- src/common/base_classes/FOCMotor.h | 6 +- src/common/foc_utils.h | 2 + src/common/lowpass_filter.cpp | 19 ++ src/common/lowpass_filter.h | 6 +- src/common/pid.cpp | 42 ++- src/common/pid.h | 5 +- src/communication/Communicator.cpp | 39 +++ src/communication/Communicator.h | 33 ++ src/current_sense/InlineCurrentSense.cpp | 12 +- .../hardware_specific/generic_mcu.cpp | 2 +- src/drivers/BLDCDriver3PWM.cpp | 22 +- src/drivers/BLDCDriver6PWM.cpp | 8 +- src/drivers/StepperDriver2PWM.cpp | 22 +- src/drivers/StepperDriver4PWM.cpp | 14 +- src/drivers/hardware_specific/due_mcu.cpp | 6 +- src/drivers/hardware_specific/esp32_mcu.cpp | 10 +- src/drivers/hardware_specific/stm32_mcu.cpp | 8 +- src/drivers/hardware_specific/teensy_mcu.cpp | 6 +- 26 files changed, 360 insertions(+), 232 deletions(-) create mode 100644 src/common/base_classes/CommunicationNode.h create mode 100644 src/communication/Communicator.cpp create mode 100644 src/communication/Communicator.h diff --git a/keywords.txt b/keywords.txt index 3bb6f48d..675c70b8 100644 --- a/keywords.txt +++ b/keywords.txt @@ -81,8 +81,6 @@ driverAlign KEYWORD2 driverSync KEYWORD2 -voltage_q KEYWORD2 -voltage_d KEYWORD2 current KEYWORD2 current_measured KEYWORD2 shaft_angle_sp KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d16091c0..a0b7b881 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -27,7 +27,7 @@ void BLDCMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Init")); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit - if( !current_sense && phase_resistance != NOT_SET ) { + if( !current_sense && _isset(phase_resistance)) { float new_voltage_limit = current_limit * (phase_resistance*1.5); // v_lim = current_lim / (3/2 phase resistance) - worst case // use it if it is less then voltage_limit set by the user voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; @@ -86,7 +86,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! - if(zero_electric_offset != NOT_SET){ + if(_isset(zero_electric_offset)){ // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW @@ -143,7 +143,7 @@ int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); // if unknown natural direction - if(sensor_direction == NOT_SET){ + if(!_isset(sensor_direction)){ // check if sensor needs zero search if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); // stop init if not found index @@ -189,7 +189,7 @@ int BLDCMotor::alignSensor() { }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); // zero electric angle not known - if(zero_electric_angle == NOT_SET){ + if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); @@ -288,7 +288,7 @@ void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; // set internal target variable - if( new_target != NOT_SET ) target = new_target; + if(_isset(new_target)) target = new_target; // get angular velocity shaft_velocity = shaftVelocity(); @@ -309,7 +309,7 @@ void BLDCMotor::move(float new_target) { // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided - if(phase_resistance == NOT_SET) voltage.q = current_sp; + if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; } @@ -322,7 +322,7 @@ void BLDCMotor::move(float new_target) { // if torque controlled through voltage control if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided - if(phase_resistance == NOT_SET) voltage.q = current_sp; + if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*1.5*phase_resistance; voltage.d = 0; } diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 761c1ebb..087617b9 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -5,6 +5,7 @@ #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" #include "common/base_classes/BLDCDriver.h" +#include "common/base_classes/CommunicationNode.h" #include "common/foc_utils.h" #include "common/time_utils.h" #include "common/defaults.h" diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 27b9ab84..8b677b84 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -108,5 +108,6 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "communication/Communicator.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 91d91763..e5906e97 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -72,7 +72,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! - if(zero_electric_offset != NOT_SET){ + if(!_isset(zero_electric_offset)){ // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW @@ -179,7 +179,7 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; // set internal target variable - if( new_target != NOT_SET ) target = new_target; + if(_isset(new_target) ) target = new_target; // get angular velocity shaft_velocity = shaftVelocity(); // choose control loop diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index ea96d3fa..f3a2e342 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -1,6 +1,8 @@ #ifndef BLDCDRIVER_H #define BLDCDRIVER_H +#include "Arduino.h" + class BLDCDriver{ public: diff --git a/src/common/base_classes/CommunicationNode.h b/src/common/base_classes/CommunicationNode.h new file mode 100644 index 00000000..a688ef48 --- /dev/null +++ b/src/common/base_classes/CommunicationNode.h @@ -0,0 +1,15 @@ +#ifndef COMNODE_h +#define COMNODE_h + +#include "Arduino.h" + +/** + * Interface class for communication + */ +class CommunicationNode{ + public: + virtual String communicate(String use_command)=0; +}; + + +#endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 0265f028..d1e73d0d 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -56,7 +56,7 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) { float FOCMotor::shaftAngle() { // if no sensor linked return previous value ( for open loop ) if(!sensor) return shaft_angle; - return sensor_direction*sensor->getAngle() - sensor_offset; + return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset; } // shaft velocity calculation float FOCMotor::shaftVelocity() { @@ -109,200 +109,171 @@ void FOCMotor::monitor() { } } -int FOCMotor::command(String user_command) { +String FOCMotor::communicate(String user_command) { // error flag - int errorFlag = 1; + String ret = ""; // if empty string - if(user_command.length() < 1) return errorFlag; + if(user_command.length() < 1) return ret; // parse command letter char cmd = user_command.charAt(0); + char sub_cmd = user_command.charAt(1); + int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; // check if get command - char GET = user_command.charAt(1) == '\n'; + char GET = user_command.charAt(value_index) == '\n'; // parse command values - float value = user_command.substring(1).toFloat(); + float value = user_command.substring(value_index).toFloat(); // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ - case 'P': // velocity P gain change - case 'I': // velocity I gain change - case 'D': // velocity D gain change - case 'R': // velocity voltage ramp change - if(monitor_port) monitor_port->print(F(" PID velocity| ")); + case 'Q': // + ret = ret + F("PID curr q| "); + if(sub_cmd == 'F') ret = ret + LPF_current_q.communicate(user_command.substring(1)); + else ret = ret + PID_current_q.communicate(user_command.substring(1)); break; - case 'F': // velocity Tf low pass filter change - if(monitor_port) monitor_port->print(F(" LPF velocity| ")); + case 'D': // + ret = ret + F("PID curr d| "); + if(sub_cmd == 'F') ret = ret + LPF_current_d.communicate(user_command.substring(1)); + else ret = ret + PID_current_d.communicate(user_command.substring(1)); break; - case 'K': // angle loop gain P change - if(monitor_port) monitor_port->print(F(" PID angle| ")); + case 'V': // + ret = ret + F("PID vel| "); + if(sub_cmd == 'F') ret = ret + LPF_velocity.communicate(user_command.substring(1)); + else ret = ret + PID_velocity.communicate(user_command.substring(1)); break; - case 'L': // velocity voltage limit change - case 'N': // angle loop gain velocity_limit change - if(monitor_port) monitor_port->print(F(" Limits| ")); + case 'A': // + ret = ret + F("PID angle| "); + if(sub_cmd == 'F') ret = ret + LPF_angle.communicate(user_command.substring(1)); + else ret = ret + P_angle.communicate(user_command.substring(1)); break; - } - - // apply the the command - switch(cmd){ - case 'P': // velocity P gain change - if(monitor_port) monitor_port->print("P: "); - if(!GET) PID_velocity.P = value; - if(monitor_port) monitor_port->println(PID_velocity.P); - break; - case 'I': // velocity I gain change - if(monitor_port) monitor_port->print("I: "); - if(!GET) PID_velocity.I = value; - if(monitor_port) monitor_port->println(PID_velocity.I); - break; - case 'D': // velocity D gain change - if(monitor_port) monitor_port->print("D: "); - if(!GET) PID_velocity.D = value; - if(monitor_port) monitor_port->println(PID_velocity.D); - break; - case 'R': // velocity voltage ramp change - if(monitor_port) monitor_port->print("volt_ramp: "); - if(!GET) PID_velocity.output_ramp = value; - if(monitor_port) monitor_port->println(PID_velocity.output_ramp); - break; - case 'L': // velocity voltage limit change - if(monitor_port) monitor_port->print("volt_limit: "); - if(!GET) { - voltage_limit = value; - PID_velocity.limit = value; + case 'L': // + ret = ret + F("Limits| "); + switch (sub_cmd){ + case 'U': // voltage limit change + ret = ret + F("voltage: "); + if(!GET) { + voltage_limit = value; + PID_current_d.limit = value; + PID_current_q.limit = value; + // change velocity pid limit if in voltage mode and no phase resistance set + if( !_isset(phase_resistance) && torque_controller==TorqueControlType::voltage) PID_velocity.limit = value; + } + ret = ret + voltage_limit; + break; + case 'C': // current limit + ret = ret + F("current: "); + if(!GET){ + current_limit = value; + // if phase resistance is set, change the voltage limit as well. + if(_isset(phase_resistance)) voltage_limit = value*phase_resistance; + // if phase resistance specified or the current control is on set the current limit to the velocity PID + if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage ) PID_velocity.limit = value; + } + ret = ret + current_limit; + break; + case 'V': // velocity limit + ret = ret + F("velocity: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + ret = ret + velocity_limit; + break; + default: + ret = ret + F("error"); + break; } - if(monitor_port) monitor_port->println(voltage_limit); - break; - case 'F': // velocity Tf low pass filter change - if(monitor_port) monitor_port->print("Tf: "); - if(!GET) LPF_velocity.Tf = value; - if(monitor_port) monitor_port->println(LPF_velocity.Tf); - break; - case 'K': // angle loop gain P change - if(monitor_port) monitor_port->print(" P: "); - if(!GET) P_angle.P = value; - if(monitor_port) monitor_port->println(P_angle.P); - break; - case 'N': // angle loop gain velocity_limit change - if(monitor_port) monitor_port->print("vel_limit: "); - if(!GET){ - velocity_limit = value; - P_angle.limit = value; - } - if(monitor_port) monitor_port->println(velocity_limit); break; case 'C': + ret = ret + F("Control: "); // change control type - if(monitor_port) monitor_port->print("Control: "); - - if(GET){ // if get command - switch(controller){ - case MotionControlType::torque: - if(monitor_port) monitor_port->println("torque"); - break; - case MotionControlType::velocity: - if(monitor_port) monitor_port->println("velocity"); - break; - case MotionControlType::angle: - if(monitor_port) monitor_port->println("angle"); - break; - case MotionControlType::velocity_openloop: - if(monitor_port) monitor_port->println("velocity openloop"); - break; - case MotionControlType::angle_openloop: - if(monitor_port) monitor_port->println("angle openloop"); - break; - } - }else{ // if set command - switch((int)value){ - case 0: - if(monitor_port) monitor_port->println("torque"); - controller = MotionControlType::torque; - break; - case 1: - if(monitor_port) monitor_port->println("velocity"); - controller = MotionControlType::velocity; - break; - case 2: - if(monitor_port) monitor_port->println("angle"); - controller = MotionControlType::angle; - break; - case 3: - if(monitor_port) monitor_port->println("velocity openloop"); - controller = MotionControlType::velocity_openloop; - break; - case 4: - if(monitor_port) monitor_port->println("angle openloop"); - controller = MotionControlType::angle_openloop; - break; - default: // not valid command - if(monitor_port) monitor_port->println("error"); - errorFlag = 0; - } + if(!GET && value >= 0 && (int)value < 5)// if set command + controller = (MotionControlType)value; + switch(controller){ + case MotionControlType::torque: + ret = ret + F("torque"); + break; + case MotionControlType::velocity: + ret = ret + F("velocity"); + break; + case MotionControlType::angle: + ret = ret + F("angle"); + break; + case MotionControlType::velocity_openloop: + ret = ret + F("velocity openloop"); + break; + case MotionControlType::angle_openloop: + ret = ret + F("angle openloop"); + break; } break; case 'T': // change control type - if(monitor_port) monitor_port->print("Torque: "); - - if(GET){ // if get command - switch(torque_controller){ - case TorqueControlType::voltage: - if(monitor_port) monitor_port->println("voltage"); - break; - case TorqueControlType::current: - if(monitor_port) monitor_port->println("current"); - break; - case TorqueControlType::foc_current: - if(monitor_port) monitor_port->println("foc current"); - break; - } - }else{ // if set command - switch((int)value){ - case 0: - if(monitor_port) monitor_port->println("voltage"); - torque_controller = TorqueControlType::voltage; - break; - case 1: - if(monitor_port) monitor_port->println("current"); - torque_controller = TorqueControlType::current; - break; - case 2: - if(monitor_port) monitor_port->println("foc current"); - torque_controller = TorqueControlType::foc_current; - break; - default: // not valid command - if(monitor_port) monitor_port->println("error"); - errorFlag = 0; - } + ret = ret + F("Torque: "); + if(!GET && value >= 0 && (int)value < 3)// if set command + torque_controller = (TorqueControlType)value; + switch(torque_controller){ + case TorqueControlType::voltage: + ret = ret + F("voltage"); + break; + case TorqueControlType::current: + ret = ret + F("current"); + break; + case TorqueControlType::foc_current: + ret = ret + F("foc current"); + break; } break; - case 'V': // get current values of the state variables + case 'E': + // enable/disable + ret = ret + F("Status: "); + if(!GET) (bool)value ? enable() : disable(); + ret = ret + enabled; + break; + case 'S': + // Sensor zero offset + ret = ret + F("Sensor | "); + switch (sub_cmd){ + case 'M': // zero offset + ret = ret + F("offset: "); + if(!GET) sensor_offset = value; + ret = ret + sensor_offset; + break; + case 'E': // electrical zero offset - not suggested to touch + ret = ret + F("el. offset: "); + if(!GET) zero_electric_angle = value; + ret = ret + zero_electric_angle; + break; + default: + ret = ret + F("error"); + break; + } + break; + case 'M': // get current values of the state variables switch((int)value){ case 0: // get voltage - if(monitor_port) monitor_port->print("Uq: "); - if(monitor_port) monitor_port->println(voltage.q); + ret = ret + F("Uq: "); + ret = ret + voltage.q; break; case 1: // get velocity - if(monitor_port) monitor_port->print("Velocity: "); - if(monitor_port) monitor_port->println(shaft_velocity); + ret = ret + F("Velocity: "); + ret = ret + shaft_velocity; break; case 2: // get angle - if(monitor_port) monitor_port->print("Angle: "); - if(monitor_port) monitor_port->println(shaft_angle); + ret = ret + F("Angle: "); + ret = ret + shaft_angle; break; case 3: // get angle - if(monitor_port) monitor_port->print("Target: "); - if(monitor_port) monitor_port->println(target); + ret = ret + F("Target: "); + ret = ret + target; break; - default: // not valid command - errorFlag = 0; } break; default: // target change - if(monitor_port) monitor_port->print("Target : "); + ret = ret + F("Target: "); target = user_command.toFloat(); - if(monitor_port) monitor_port->println(target); + ret = ret + target; } - // return 0 if error and 1 if ok - return errorFlag; -} \ No newline at end of file + + return ret; +} diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 89cedf71..f0365bf0 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "Sensor.h" #include "CurrentSense.h" +#include "CommunicationNode.h" #include "../time_utils.h" #include "../foc_utils.h" @@ -44,7 +45,7 @@ enum FOCModulationType{ /** Generic motor class */ -class FOCMotor +class FOCMotor: public CommunicationNode { public: /** @@ -161,6 +162,7 @@ class FOCMotor 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 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 // sensor related variabels float sensor_offset; //!< user defined sensor zero offset @@ -224,7 +226,7 @@ class FOCMotor * * returns 0 for error or 1 for executed command */ - int command(String command); + String communicate(String command) override; /** * Sensor link: diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 7f6ae0f1..06978424 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -8,6 +8,7 @@ #define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) +#define _isset(a) ( (a) != (NOT_SET) ) // utility defines #define _2_SQRT3 1.15470053838 @@ -86,4 +87,5 @@ float _electricalAngle(float shaft_angle, int pole_pairs); * @param value - number */ float _sqrtApprox(float value); + #endif \ No newline at end of file diff --git a/src/common/lowpass_filter.cpp b/src/common/lowpass_filter.cpp index 9bf4ab7a..35cad62d 100644 --- a/src/common/lowpass_filter.cpp +++ b/src/common/lowpass_filter.cpp @@ -22,4 +22,23 @@ float LowPassFilter::operator() (float x) y_prev = y; timestamp_prev = timestamp; return y; +} + +String LowPassFilter::communicate(String user_cmd){ + String ret = ""; + char cmd = user_cmd.charAt(0); + char GET = user_cmd.charAt(1) == '\n'; + float value = user_cmd.substring(1).toFloat(); + + switch (cmd){ + case 'F': // Tf value change + ret = ret + "Tf: "; + if(!GET) Tf = value; + ret = ret + Tf; + break; + default: + ret = ret + F("error"); + break; + } + return ret; } \ No newline at end of file diff --git a/src/common/lowpass_filter.h b/src/common/lowpass_filter.h index 7c51be54..00ea00cc 100644 --- a/src/common/lowpass_filter.h +++ b/src/common/lowpass_filter.h @@ -4,12 +4,12 @@ #include "time_utils.h" #include "foc_utils.h" - +#include "base_classes/CommunicationNode.h" /** * Low pass filter class */ -class LowPassFilter +class LowPassFilter: public CommunicationNode { public: /** @@ -21,6 +21,8 @@ class LowPassFilter float operator() (float x); float Tf; //!< Low pass filter time constant + String communicate(String user_command) override; + protected: unsigned long timestamp_prev; //!< Last execution timestamp float y_prev; //!< filtered value in previous execution step diff --git a/src/common/pid.cpp b/src/common/pid.cpp index eb154230..acfe0b15 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -46,11 +46,51 @@ float PIDController::operator() (float error){ 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; error_prev = error; timestamp_prev = timestamp_now; return output; +} + + +String PIDController::communicate(String user_cmd){ + String ret = ""; + char cmd = user_cmd.charAt(0); + char GET = user_cmd.charAt(1) == '\n'; + float value = user_cmd.substring(1).toFloat(); + + switch (cmd){ + case 'P': // P gain change + ret = ret + "P: "; + if(!GET) P = value; + ret = ret + P; + break; + case 'I': // I gain change + ret = ret + "I: "; + if(!GET) I = value; + ret = ret + I; + break; + case 'D': // D gain change + ret = ret + "D: "; + if(!GET) D = value; + ret = ret + D; + break; + case 'R': // ramp change + ret = ret + "ramp: "; + if(!GET) output_ramp = value; + ret = ret + output_ramp; + break; + case 'L': // limit change + ret = ret + "limit: "; + if(!GET) limit = value; + ret = ret + limit; + break; + default: + ret = ret + F("error"); + break; + } + return ret; // not well handled } \ No newline at end of file diff --git a/src/common/pid.h b/src/common/pid.h index 39622ace..455db429 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -4,11 +4,12 @@ #include "time_utils.h" #include "foc_utils.h" +#include "base_classes/CommunicationNode.h" /** * PID controller class */ -class PIDController +class PIDController: public CommunicationNode { public: /** @@ -30,6 +31,8 @@ class PIDController float output_ramp; //!< Maximum speed of change of the output value float limit; //!< Maximum output value + String communicate(String user_command) override; + protected: float integral_prev; //!< last integral component value float error_prev; //!< last tracking error value diff --git a/src/communication/Communicator.cpp b/src/communication/Communicator.cpp new file mode 100644 index 00000000..c4d210ea --- /dev/null +++ b/src/communication/Communicator.cpp @@ -0,0 +1,39 @@ +#include "Communicator.h" + + + +Communicator::Communicator(HardwareSerial& serial){ + com_port = &serial; +} + +/** + CurrentSense linking method +*/ +void Communicator::addNode(CommunicationNode* _node, char id) { + node_list[node_count] = _node; + node_ids[node_count++] = id; +} + +void Communicator::run(){ + // a string to hold incoming data + while (com_port->available()) { + // get the new byte: + char inChar = (char)com_port->read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + // execute the user command + char id = received_chars.charAt(0); + for(int i=0; iprintln(node_list[i]->communicate(received_chars.substring(1))); + break; + } + } + // reset the command buffer + received_chars = ""; + } + } +} + diff --git a/src/communication/Communicator.h b/src/communication/Communicator.h new file mode 100644 index 00000000..e4d9f498 --- /dev/null +++ b/src/communication/Communicator.h @@ -0,0 +1,33 @@ +#ifndef COMMANDS_H +#define COMMANDS_H + +#include "Arduino.h" +#include "../common/base_classes/CommunicationNode.h" + +/** + Serial communiactor +*/ +class Communicator +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + Communicator(HardwareSerial &serial); + + void run(); + void addNode(CommunicationNode* node, char id = 0); + + // monitoring functions + HardwareSerial* com_port; //!< Serial terminal variable if provided + + private: + CommunicationNode* node_list[10]; //!< Dictionary of nodes + char node_ids[10]; //!< Dictionary of nodes + int node_count = 0; + String received_chars = ""; + +}; + + +#endif diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index cf06774a..ed395325 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -36,12 +36,12 @@ void InlineCurrentSense::calibrateOffsets(){ for (int i = 0; i < 500; i++) { offset_ia += _readADCVoltage(pinA); offset_ib += _readADCVoltage(pinB); - if(pinC != NOT_SET) offset_ic += _readADCVoltage(pinC); + if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); } // calculate the mean offsets offset_ia = offset_ia / 500.0; offset_ib = offset_ib / 500.0; - if(pinC != NOT_SET) offset_ic = offset_ic / 500.0; + if(_isset(pinC)) offset_ic = offset_ic / 500.0; } // read all three phase currents (if possible 2 or 3) @@ -49,7 +49,7 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - current.c = (pinC == NOT_SET) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. @@ -95,7 +95,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ pinB = tmp_pinA; gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched - }else if(pinC != NOT_SET && ac_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; pinA = pinC; @@ -131,7 +131,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ pinA = tmp_pinB; gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched - }else if(pinC != NOT_SET && bc_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; pinB = pinC; @@ -144,7 +144,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ } // if phase C measured - if(pinC != NOT_SET){ + if(_isset(pinC)){ // set phase B active and phases A and C down driver->setPwm(0, 0, voltage); _delay(200); diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 667bb8a4..423774c2 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -43,7 +43,7 @@ float _readADCVoltage(const int pinA){ void _configureADC(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); - if( pinC != NOT_SET ) pinMode(pinC, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 0841870d..87eb7f5b 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,9 +20,9 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( enableA_pin != NOT_SET ) digitalWrite(enableA_pin, HIGH); - if ( enableB_pin != NOT_SET ) digitalWrite(enableB_pin, HIGH); - if ( enableC_pin != NOT_SET ) digitalWrite(enableC_pin, HIGH); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, HIGH); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, HIGH); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, HIGH); // set zero to PWM setPwm(0,0,0); } @@ -33,9 +33,9 @@ void BLDCDriver3PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( enableA_pin != NOT_SET ) digitalWrite(enableA_pin, LOW); - if ( enableB_pin != NOT_SET ) digitalWrite(enableB_pin, LOW); - if ( enableC_pin != NOT_SET ) digitalWrite(enableC_pin, LOW); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, LOW); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, LOW); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, LOW); } @@ -48,13 +48,13 @@ int BLDCDriver3PWM::init() { pinMode(pwmA, OUTPUT); pinMode(pwmB, OUTPUT); pinMode(pwmC, OUTPUT); - if(enableA_pin != NOT_SET) pinMode(enableA_pin, OUTPUT); - if(enableB_pin != NOT_SET) pinMode(enableB_pin, OUTPUT); - if(enableC_pin != NOT_SET) pinMode(enableC_pin, OUTPUT); + if( _isset(enableA_pin)) pinMode(enableA_pin, OUTPUT); + if( _isset(enableB_pin)) pinMode(enableB_pin, OUTPUT); + if( _isset(enableC_pin)) pinMode(enableC_pin, OUTPUT); // sanity check for the voltage limit configuration - if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if(!_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu @@ -67,7 +67,7 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed - if(enableA_pin != NOT_SET && enableB_pin != NOT_SET && enableC_pin != NOT_SET ){ + if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); digitalWrite(enableB_pin, sb == _HIGH_IMPEDANCE ? LOW : HIGH); digitalWrite(enableC_pin, sc == _HIGH_IMPEDANCE ? LOW : HIGH); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 2d9cb256..cd0d842b 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 ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, 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 ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, LOW); } @@ -51,11 +51,11 @@ int BLDCDriver6PWM::init() { pinMode(pwmA_l, OUTPUT); pinMode(pwmB_l, OUTPUT); pinMode(pwmC_l, OUTPUT); - if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT); // sanity check for the voltage limit configuration - if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if( _isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // configure 6pwm // hardware specific function - depending on driver and mcu diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a08c5e6b..7ca7cc31 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -42,8 +42,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); // set zero to PWM setPwm(0,0); } @@ -54,8 +54,8 @@ void StepperDriver2PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); } @@ -69,14 +69,14 @@ int StepperDriver2PWM::init() { pinMode(pwm2, OUTPUT); pinMode(dir1a, OUTPUT); pinMode(dir2a, OUTPUT); - if(dir1b != NOT_SET ) pinMode(dir1b, OUTPUT); - if(dir2b != NOT_SET ) pinMode(dir2b, OUTPUT); + if( _isset(dir1b) ) pinMode(dir1b, OUTPUT); + if( _isset(dir2b) ) pinMode(dir2b, OUTPUT); - if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); - if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT); + if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT); // sanity check for the voltage limit configuration - if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu @@ -97,10 +97,10 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); - if(dir1b != NOT_SET) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); - if(dir2b != NOT_SET) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); + if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 28e2dcae..428e0ad1 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -20,8 +20,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); // set zero to PWM setPwm(0,0); } @@ -32,8 +32,8 @@ void StepperDriver4PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); - if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); } @@ -47,11 +47,11 @@ int StepperDriver4PWM::init() { pinMode(pwm1B, OUTPUT); pinMode(pwm2A, OUTPUT); pinMode(pwm2B, OUTPUT); - if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); - if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT); + if (_isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT); // sanity check for the voltage limit configuration - if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index bb8e6f4b..27097301 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -297,7 +297,7 @@ void TC8_Handler() // - BLDC motor - 3PWM setting // - hardware specific void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 35000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; @@ -314,7 +314,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int //- Stepper driver - 2PWM setting // - hardware specific void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; @@ -329,7 +329,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // - Stepper motor - 4PWM setting // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 35000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 983d4d54..7350316d 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -106,7 +106,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm 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 - if (dead_zone != NOT_SET){ + if (_isset(dead_zone)){ // dead zone is configured float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); @@ -170,7 +170,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // supports Arudino/ATmega328, STM32 and ESP32 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + 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 stepper_2pwm_motor_slots_t m_slot = {}; @@ -216,7 +216,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // supports Arudino/ATmega328, STM32 and ESP32 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + 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 bldc_3pwm_motor_slots_t m_slot = {}; @@ -261,7 +261,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + 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 stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting @@ -364,7 +364,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + 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 bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index c6298d1b..3f213711 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -190,7 +190,7 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const // - Stepper motor - 2PWM setting // - hardware speciffic void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; @@ -206,7 +206,7 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; @@ -222,7 +222,7 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; @@ -273,7 +273,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 2a584876..e95f2066 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -13,7 +13,7 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); @@ -23,7 +23,7 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); @@ -34,7 +34,7 @@ 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 || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); From a91740685e8eae2c86b4017083e4f3df9a104cd5 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 17 Feb 2021 19:53:26 +0100 Subject: [PATCH 095/749] FEAT new communication protocol --- README.md | 4 + keywords.txt | 4 + src/BLDCMotor.h | 1 - src/SimpleFOC.h | 2 +- src/common/base_classes/FOCMotor.cpp | 168 ----------------- src/common/base_classes/FOCMotor.h | 50 +---- src/common/lowpass_filter.cpp | 19 -- src/common/lowpass_filter.h | 5 +- src/common/pid.cpp | 39 ---- src/common/pid.h | 5 +- src/communication/Commander.cpp | 272 +++++++++++++++++++++++++++ src/communication/Commander.h | 85 +++++++++ src/communication/Communicator.cpp | 39 ---- src/communication/Communicator.h | 33 ---- src/communication/commands.h | 30 +++ 15 files changed, 400 insertions(+), 356 deletions(-) create mode 100644 src/communication/Commander.cpp create mode 100644 src/communication/Commander.h delete mode 100644 src/communication/Communicator.cpp delete mode 100644 src/communication/Communicator.h create mode 100644 src/communication/commands.h diff --git a/README.md b/README.md index 258358f1..4b3a2a6b 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,10 @@ Therefore this is an attempt to: > - 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 > > BEWARE 📢 slight API changes included > - `ControlType` renamed into `MotionControlType` diff --git a/keywords.txt b/keywords.txt index 675c70b8..2eab2f69 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ PIDController KEYWORD1 LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 CurrentSense KEYWORD1 +Commander KEYWORD1 initFOC KEYWORD2 @@ -44,6 +45,7 @@ LPF_velocity KEYWORD2 LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 +LPF_angle KEYWORD2 MotionControlType KEYWORD1 TorqueControlType KEYWORD1 @@ -79,6 +81,8 @@ getCurrent KEYWORD2 setPwm KEYWORD2 driverAlign KEYWORD2 driverSync KEYWORD2 +add KEYWORD2 +run KEYWORD2 current KEYWORD2 diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 087617b9..761c1ebb 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -5,7 +5,6 @@ #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" #include "common/base_classes/BLDCDriver.h" -#include "common/base_classes/CommunicationNode.h" #include "common/foc_utils.h" #include "common/time_utils.h" #include "common/defaults.h" diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 8b677b84..5b1c4ee0 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -108,6 +108,6 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" -#include "communication/Communicator.h" +#include "communication/Commander.h" #endif diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1e73d0d..193a7ad1 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -109,171 +109,3 @@ void FOCMotor::monitor() { } } -String FOCMotor::communicate(String user_command) { - // error flag - String ret = ""; - // if empty string - if(user_command.length() < 1) return ret; - - // parse command letter - char cmd = user_command.charAt(0); - char sub_cmd = user_command.charAt(1); - int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; - // check if get command - char GET = user_command.charAt(value_index) == '\n'; - // parse command values - float value = user_command.substring(value_index).toFloat(); - - // a bit of optimisation of variable memory for Arduino UNO (atmega328) - switch(cmd){ - case 'Q': // - ret = ret + F("PID curr q| "); - if(sub_cmd == 'F') ret = ret + LPF_current_q.communicate(user_command.substring(1)); - else ret = ret + PID_current_q.communicate(user_command.substring(1)); - break; - case 'D': // - ret = ret + F("PID curr d| "); - if(sub_cmd == 'F') ret = ret + LPF_current_d.communicate(user_command.substring(1)); - else ret = ret + PID_current_d.communicate(user_command.substring(1)); - break; - case 'V': // - ret = ret + F("PID vel| "); - if(sub_cmd == 'F') ret = ret + LPF_velocity.communicate(user_command.substring(1)); - else ret = ret + PID_velocity.communicate(user_command.substring(1)); - break; - case 'A': // - ret = ret + F("PID angle| "); - if(sub_cmd == 'F') ret = ret + LPF_angle.communicate(user_command.substring(1)); - else ret = ret + P_angle.communicate(user_command.substring(1)); - break; - case 'L': // - ret = ret + F("Limits| "); - switch (sub_cmd){ - case 'U': // voltage limit change - ret = ret + F("voltage: "); - if(!GET) { - voltage_limit = value; - PID_current_d.limit = value; - PID_current_q.limit = value; - // change velocity pid limit if in voltage mode and no phase resistance set - if( !_isset(phase_resistance) && torque_controller==TorqueControlType::voltage) PID_velocity.limit = value; - } - ret = ret + voltage_limit; - break; - case 'C': // current limit - ret = ret + F("current: "); - if(!GET){ - current_limit = value; - // if phase resistance is set, change the voltage limit as well. - if(_isset(phase_resistance)) voltage_limit = value*phase_resistance; - // if phase resistance specified or the current control is on set the current limit to the velocity PID - if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage ) PID_velocity.limit = value; - } - ret = ret + current_limit; - break; - case 'V': // velocity limit - ret = ret + F("velocity: "); - if(!GET){ - velocity_limit = value; - P_angle.limit = value; - } - ret = ret + velocity_limit; - break; - default: - ret = ret + F("error"); - break; - } - break; - case 'C': - ret = ret + F("Control: "); - // change control type - if(!GET && value >= 0 && (int)value < 5)// if set command - controller = (MotionControlType)value; - switch(controller){ - case MotionControlType::torque: - ret = ret + F("torque"); - break; - case MotionControlType::velocity: - ret = ret + F("velocity"); - break; - case MotionControlType::angle: - ret = ret + F("angle"); - break; - case MotionControlType::velocity_openloop: - ret = ret + F("velocity openloop"); - break; - case MotionControlType::angle_openloop: - ret = ret + F("angle openloop"); - break; - } - break; - case 'T': - // change control type - ret = ret + F("Torque: "); - if(!GET && value >= 0 && (int)value < 3)// if set command - torque_controller = (TorqueControlType)value; - switch(torque_controller){ - case TorqueControlType::voltage: - ret = ret + F("voltage"); - break; - case TorqueControlType::current: - ret = ret + F("current"); - break; - case TorqueControlType::foc_current: - ret = ret + F("foc current"); - break; - } - break; - case 'E': - // enable/disable - ret = ret + F("Status: "); - if(!GET) (bool)value ? enable() : disable(); - ret = ret + enabled; - break; - case 'S': - // Sensor zero offset - ret = ret + F("Sensor | "); - switch (sub_cmd){ - case 'M': // zero offset - ret = ret + F("offset: "); - if(!GET) sensor_offset = value; - ret = ret + sensor_offset; - break; - case 'E': // electrical zero offset - not suggested to touch - ret = ret + F("el. offset: "); - if(!GET) zero_electric_angle = value; - ret = ret + zero_electric_angle; - break; - default: - ret = ret + F("error"); - break; - } - break; - case 'M': // get current values of the state variables - switch((int)value){ - case 0: // get voltage - ret = ret + F("Uq: "); - ret = ret + voltage.q; - break; - case 1: // get velocity - ret = ret + F("Velocity: "); - ret = ret + shaft_velocity; - break; - case 2: // get angle - ret = ret + F("Angle: "); - ret = ret + shaft_angle; - break; - case 3: // get angle - ret = ret + F("Target: "); - ret = ret + target; - break; - } - break; - default: // target change - ret = ret + F("Target: "); - target = user_command.toFloat(); - ret = ret + target; - } - - return ret; -} diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index f0365bf0..603ec024 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -4,7 +4,6 @@ #include "Arduino.h" #include "Sensor.h" #include "CurrentSense.h" -#include "CommunicationNode.h" #include "../time_utils.h" #include "../foc_utils.h" @@ -45,7 +44,7 @@ enum FOCModulationType{ /** Generic motor class */ -class FOCMotor: public CommunicationNode +class FOCMotor { public: /** @@ -182,52 +181,7 @@ class FOCMotor: public CommunicationNode * significantly slowing the execution down!!!! */ void monitor(); - - /** - * Function setting the configuration parameters of the motor, target value of the control loop - * and outputing them to the monitoring port( if available ) : - * - configure PID controller constants - * - change motion control loops - * - monitor motor variabels - * - set target values - * - check all the configuration values - * - * To check the config value just enter the command letter. - * For example: - * - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - * - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PI controller P gain - * - I: velocity PI controller I gain - * - L: velocity PI controller voltage limit - * - R: velocity PI controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: angle - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * - * - Look into the documentation (docs.simplefoc.com) for more information. - * - * @param command String containing the user command - * - * returns 0 for error or 1 for executed command - */ - String communicate(String command) override; - + /** * Sensor link: * - Encoder diff --git a/src/common/lowpass_filter.cpp b/src/common/lowpass_filter.cpp index 35cad62d..4a643744 100644 --- a/src/common/lowpass_filter.cpp +++ b/src/common/lowpass_filter.cpp @@ -23,22 +23,3 @@ float LowPassFilter::operator() (float x) timestamp_prev = timestamp; return y; } - -String LowPassFilter::communicate(String user_cmd){ - String ret = ""; - char cmd = user_cmd.charAt(0); - char GET = user_cmd.charAt(1) == '\n'; - float value = user_cmd.substring(1).toFloat(); - - switch (cmd){ - case 'F': // Tf value change - ret = ret + "Tf: "; - if(!GET) Tf = value; - ret = ret + Tf; - break; - default: - ret = ret + F("error"); - break; - } - return ret; -} \ No newline at end of file diff --git a/src/common/lowpass_filter.h b/src/common/lowpass_filter.h index 00ea00cc..1f24e80d 100644 --- a/src/common/lowpass_filter.h +++ b/src/common/lowpass_filter.h @@ -4,12 +4,11 @@ #include "time_utils.h" #include "foc_utils.h" -#include "base_classes/CommunicationNode.h" /** * Low pass filter class */ -class LowPassFilter: public CommunicationNode +class LowPassFilter { public: /** @@ -21,8 +20,6 @@ class LowPassFilter: public CommunicationNode float operator() (float x); float Tf; //!< Low pass filter time constant - String communicate(String user_command) override; - protected: unsigned long timestamp_prev; //!< Last execution timestamp float y_prev; //!< filtered value in previous execution step diff --git a/src/common/pid.cpp b/src/common/pid.cpp index acfe0b15..a910556f 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -55,42 +55,3 @@ float PIDController::operator() (float error){ return output; } - -String PIDController::communicate(String user_cmd){ - String ret = ""; - char cmd = user_cmd.charAt(0); - char GET = user_cmd.charAt(1) == '\n'; - float value = user_cmd.substring(1).toFloat(); - - switch (cmd){ - case 'P': // P gain change - ret = ret + "P: "; - if(!GET) P = value; - ret = ret + P; - break; - case 'I': // I gain change - ret = ret + "I: "; - if(!GET) I = value; - ret = ret + I; - break; - case 'D': // D gain change - ret = ret + "D: "; - if(!GET) D = value; - ret = ret + D; - break; - case 'R': // ramp change - ret = ret + "ramp: "; - if(!GET) output_ramp = value; - ret = ret + output_ramp; - break; - case 'L': // limit change - ret = ret + "limit: "; - if(!GET) limit = value; - ret = ret + limit; - break; - default: - ret = ret + F("error"); - break; - } - return ret; // not well handled -} \ No newline at end of file diff --git a/src/common/pid.h b/src/common/pid.h index 455db429..39622ace 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -4,12 +4,11 @@ #include "time_utils.h" #include "foc_utils.h" -#include "base_classes/CommunicationNode.h" /** * PID controller class */ -class PIDController: public CommunicationNode +class PIDController { public: /** @@ -31,8 +30,6 @@ class PIDController: public CommunicationNode float output_ramp; //!< Maximum speed of change of the output value float limit; //!< Maximum output value - String communicate(String user_command) override; - protected: float integral_prev; //!< last integral component value float error_prev; //!< last tracking error value diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp new file mode 100644 index 00000000..a968fbbe --- /dev/null +++ b/src/communication/Commander.cpp @@ -0,0 +1,272 @@ +#include "Commander.h" + + +Commander::Commander(HardwareSerial& serial){ + com_port = &serial; +} + +void Commander::add(char id, CommandCallback onCommand){ + call_list[call_count] = onCommand; + call_ids[call_count] = id; + call_count++; +} + +void Commander::run(){ + // a string to hold incoming data + while (com_port->available()) { + // get the new byte: + char inChar = (char)com_port->read(); + // add it to the string buffer: + received_chars += inChar; + // end of user input + if (inChar == '\n') { + // execute the user command + char id = received_chars.charAt(0); + for(int i=0; i < call_count; i++){ + if(id == call_ids[i]){ + call_list[i](received_chars.substring(1)); + break; + } + } + // reset the command buffer + received_chars = ""; + } + } +} + +void Commander::motor(FOCMotor* motor, String user_command) { + // if empty string + if( user_command.length() < 1 ) return; + + // parse command letter + char cmd = user_command.charAt(0); + char sub_cmd = user_command.charAt(1); + int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; + // check if get command + bool GET = user_command.charAt(value_index) == '\n'; + // parse command values + float value = user_command.substring(value_index).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case CMD_C_Q_PID: // + com_port->print(F("PID curr q| ")); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, user_command.substring(1)); + else pid(&motor->PID_current_q,user_command.substring(1)); + break; + case CMD_C_D_PID: // + com_port->print(F("PID curr d| ")); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, user_command.substring(1)); + else pid(&motor->PID_current_d, user_command.substring(1)); + break; + case CMD_V_PID: // + com_port->print(F("PID vel| ")); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, user_command.substring(1)); + else pid(&motor->PID_velocity, user_command.substring(1)); + break; + case CMD_A_PID: // + com_port->print(F("PID angle| ")); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, user_command.substring(1)); + else pid(&motor->P_angle, user_command.substring(1)); + break; + case CMD_LIMITS: // + com_port->print(F("Limits| ")); + switch (sub_cmd){ + case SCMD_LIM_VOLT: // voltage limit change + com_port->print(F("volt: ")); + if(!GET) { + motor->voltage_limit = value; + motor->PID_current_d.limit = value; + motor->PID_current_q.limit = value; + // change velocity pid limit if in voltage mode and no phase resistance set + if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value; + } + com_port->println(motor->voltage_limit); + break; + case SCMD_LIM_CURR: // current limit + com_port->print(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; + } + com_port->println(motor->current_limit); + break; + case SCMD_LIM_VEL: // velocity limit + com_port->print(F("vel: ")); + if(!GET){ + motor->velocity_limit = value; + motor->P_angle.limit = value; + } + com_port->println(motor->velocity_limit); + break; + default: + com_port->println(F("err")); + break; + } + break; + case CMD_MOTION_TYPE: + com_port->print(F("Motion: ")); + // change control type + if(!GET && value >= 0 && (int)value < 5)// if set command + motor->controller = (MotionControlType)value; + switch(motor->controller){ + case MotionControlType::torque: + com_port->println(F("torque")); + break; + case MotionControlType::velocity: + com_port->println(F("vel")); + break; + case MotionControlType::angle: + com_port->println(F("angle")); + break; + case MotionControlType::velocity_openloop: + com_port->println(F("vel open")); + break; + case MotionControlType::angle_openloop: + com_port->println(F("angle open")); + break; + } + break; + case CMD_TORQUE_TYPE: + // change control type + com_port->print(F("Torque: ")); + if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command + motor->torque_controller = (TorqueControlType)value; + switch(motor->torque_controller){ + case TorqueControlType::voltage: + com_port->println(F("volt")); + break; + case TorqueControlType::current: + com_port->println(F("curr")); + break; + case TorqueControlType::foc_current: + com_port->println(F("foc")); + break; + } + break; + case CMD_STATUS: + // enable/disable + com_port->print(F("Status: ")); + if(!GET) (bool)value ? motor->enable() : motor->disable(); + com_port->println(motor->enabled); + break; + case CMD_RESIST: + // enable/disable + com_port->print(F("R phase: ")); + if(!GET){ + motor->phase_resistance = value; + if(motor->torque_controller==TorqueControlType::voltage){ + motor->voltage_limit = motor->current_limit*value; + motor->PID_velocity.limit= motor->current_limit; + } + } + com_port->println(_isset(motor->phase_resistance) ? motor->phase_resistance : 0 ); + break; + case CMD_SENSOR: + // Sensor zero offset + com_port->print(F("Sensor | ")); + switch (sub_cmd){ + case SCMD_SENS_MECH_OFFSET: // zero offset + com_port->print(F("offset: ")); + if(!GET) motor->sensor_offset = value; + com_port->println(motor->sensor_offset); + break; + case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch + com_port->print(F("el. offset: ")); + if(!GET) motor->zero_electric_angle = value; + com_port->println(motor->zero_electric_angle); + break; + default: + com_port->println(F("err")); + break; + } + break; + case CMD_MONITOR: // get current values of the state variables + switch((uint8_t)value){ + case 0: // get voltage + com_port->print(F("Uq: ")); + com_port->println(motor->voltage.q); + break; + case 1: // get velocity + com_port->print(F("vel: ")); + com_port->println(motor->shaft_velocity); + break; + case 2: // get angle + com_port->print(F("Angle: ")); + com_port->println(motor->shaft_angle); + break; + case 3: // get angle + com_port->print(F("target: ")); + com_port->println(motor->target); + break; + } + break; + default: // target change + com_port->print(F("Target: ")); + motor->target = user_command.toFloat(); + com_port->println(motor->target); + } +} + +void Commander::pid(PIDController* pid, String user_cmd){ + char cmd = user_cmd.charAt(0); + bool GET = user_cmd.charAt(1) == '\n'; + float value = user_cmd.substring(1).toFloat(); + + switch (cmd){ + case SCMD_PID_P: // P gain change + com_port->print("P: "); + if(!GET) pid->P = value; + com_port->println(pid->P); + break; + case SCMD_PID_I: // I gain change + com_port->print("I: "); + if(!GET) pid->I = value; + com_port->println(pid->I); + break; + case SCMD_PID_D: // D gain change + com_port->print("D: "); + if(!GET) pid->D = value; + com_port->println(pid->D); + break; + case SCMD_PID_RAMP: // ramp change + com_port->print("ramp: "); + if(!GET) pid->output_ramp = value; + com_port->println(pid->output_ramp); + break; + case SCMD_PID_LIM: // limit change + com_port->print("limit: "); + if(!GET) pid->limit = value; + com_port->println(pid->limit); + break; + default: + com_port->println(F("err")); + break; + } +} + +void Commander::lpf(LowPassFilter* lpf, String user_cmd){ + char cmd = user_cmd.charAt(0); + bool GET = user_cmd.charAt(1) == '\n'; + float value = user_cmd.substring(1).toFloat(); + + switch (cmd){ + case SCMD_LPF_TF: // Tf value change + com_port->print("Tf: "); + if(!GET) lpf->Tf = value; + com_port->println(lpf->Tf); + break; + default: + com_port->println(F("err")); + break; + } +} + +void Commander::scalar(float* value, String user_cmd){ + bool GET = user_cmd.charAt(0) == '\n'; + if(!GET) *value = user_cmd.toFloat(); + com_port->println(*value); +} \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h new file mode 100644 index 00000000..30d5edd0 --- /dev/null +++ b/src/communication/Commander.h @@ -0,0 +1,85 @@ +#ifndef COMMANDS_H +#define COMMANDS_H + +#include "Arduino.h" +#include "../common/base_classes/FOCMotor.h" +#include "../common/pid.h" +#include "../common/lowpass_filter.h" +#include "commands.h" + + +typedef void (* CommandCallback)(String); + +class Commander +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + Commander(HardwareSerial &serial); + + void run(); + + void add(char id , CommandCallback onCommand); + + // monitoring functions + HardwareSerial* com_port; //!< Serial terminal variable if provided + + /** + * Function setting the configuration parameters of the motor, target value of the control loop + * and outputing them to the monitoring port( if available ) : + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: + * - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: + * - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PI controller P gain + * - I: velocity PI controller I gain + * - L: velocity PI controller voltage limit + * - R: velocity PI controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current angle + * - 3: current target value + * + * - Look into the documentation (docs.simplefoc.com) for more information. + * + * @param command String containing the user command + * + * returns 0 for error or 1 for executed command + */ + void motor(FOCMotor* motor, String user_cmd); + void lpf(LowPassFilter* lpf, String user_cmd); + void pid(PIDController* pid, String user_cmd); + void scalar(float* value, String user_cmd); + + private: + CommandCallback call_list[20]; + char call_ids[20]; //!< Dictionary of nodes + int call_count = 0; + String received_chars = ""; + +}; + + +#endif diff --git a/src/communication/Communicator.cpp b/src/communication/Communicator.cpp deleted file mode 100644 index c4d210ea..00000000 --- a/src/communication/Communicator.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "Communicator.h" - - - -Communicator::Communicator(HardwareSerial& serial){ - com_port = &serial; -} - -/** - CurrentSense linking method -*/ -void Communicator::addNode(CommunicationNode* _node, char id) { - node_list[node_count] = _node; - node_ids[node_count++] = id; -} - -void Communicator::run(){ - // a string to hold incoming data - while (com_port->available()) { - // get the new byte: - char inChar = (char)com_port->read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - // execute the user command - char id = received_chars.charAt(0); - for(int i=0; iprintln(node_list[i]->communicate(received_chars.substring(1))); - break; - } - } - // reset the command buffer - received_chars = ""; - } - } -} - diff --git a/src/communication/Communicator.h b/src/communication/Communicator.h deleted file mode 100644 index e4d9f498..00000000 --- a/src/communication/Communicator.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef COMMANDS_H -#define COMMANDS_H - -#include "Arduino.h" -#include "../common/base_classes/CommunicationNode.h" - -/** - Serial communiactor -*/ -class Communicator -{ - public: - /** - * Default constructor - setting all variabels to default values - */ - Communicator(HardwareSerial &serial); - - void run(); - void addNode(CommunicationNode* node, char id = 0); - - // monitoring functions - HardwareSerial* com_port; //!< Serial terminal variable if provided - - private: - CommunicationNode* node_list[10]; //!< Dictionary of nodes - char node_ids[10]; //!< Dictionary of nodes - int node_count = 0; - String received_chars = ""; - -}; - - -#endif diff --git a/src/communication/commands.h b/src/communication/commands.h new file mode 100644 index 00000000..7c7969b4 --- /dev/null +++ b/src/communication/commands.h @@ -0,0 +1,30 @@ +#ifndef COMMANDS_h +#define COMMANDS_h + +// list o commands + #define CMD_C_D_PID 'D' + #define CMD_C_Q_PID 'Q' + #define CMD_V_PID 'V' + #define CMD_A_PID 'A' + #define CMD_STATUS 'E' + #define CMD_LIMITS 'L' + #define CMD_MOTION_TYPE 'C' + #define CMD_TORQUE_TYPE 'T' + #define CMD_SENSOR 'S' + #define CMD_MONITOR 'M' + #define CMD_RESIST 'R' + +// subcomands + #define SCMD_PID_P 'P' + #define SCMD_PID_I 'I' + #define SCMD_PID_D 'D' + #define SCMD_PID_RAMP 'R' + #define SCMD_PID_LIM 'L' + #define SCMD_LPF_TF 'F' + #define SCMD_LIM_CURR 'C' + #define SCMD_LIM_VOLT 'U' + #define SCMD_LIM_VEL 'V' + #define SCMD_SENS_MECH_OFFSET 'M' + #define SCMD_SENS_ELEC_OFFSET 'E' + +#endif \ No newline at end of file From 4c6061c2bd5892bdffa2958afbabf80465c74dd1 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 17 Feb 2021 20:12:39 +0100 Subject: [PATCH 096/749] FEAT minimal modifiaction of examples to pass the compile --- .../full_control_serial.ino | 39 +++++-------------- .../full_control_serial.ino | 37 ++++-------------- .../full_control_serial.ino | 39 +++++-------------- .../full_control_serial.ino | 36 +++++------------ .../full_control_serial.ino | 35 ++++------------- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 4 +- src/BLDCMotor.cpp | 8 ++-- 7 files changed, 50 insertions(+), 148 deletions(-) diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 4f6ede28..3e1f3357 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -36,6 +36,11 @@ Encoder encoder = Encoder(2, 3, 8192); void doA(){encoder.handleA();} void doB(){encoder.handleB();} + +// commander interface +Commander command = Commander(Serial); +void onA(String cmd){ command.motor(&motor, cmd); } + void setup() { // initialize encoder sensor hardware @@ -99,6 +104,8 @@ void setup() { // set the inital target value motor.target = 2; + // define the motor id + command.add('A', onA); Serial.println(F("Full control example: ")); Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); @@ -119,33 +126,5 @@ void loop() { motor.move(); // user communication - motor.command(serialReceiveUserCommand()); -} - -// utility function enabling serial communication the user -String serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - String command = ""; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - - // end of user input - if (inChar == '\n') { - - // execute the user command - command = received_chars; - - // reset the command buffer - received_chars = ""; - } - } - return command; -} - + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 627c2c43..2ac51d20 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -39,6 +39,11 @@ Encoder encoder = Encoder(2, 3, 8192); void doA(){encoder.handleA();} void doB(){encoder.handleB();} + +// commander interface +Commander command = Commander(Serial); +void onA(String cmd){ command.motor(&motor, cmd); } + void setup() { // initialize encoder sensor hardware @@ -100,6 +105,8 @@ void setup() { // set the inital target value motor.target = 2; + // define the motor id + command.add('A', onA); Serial.println(F("Full control example: ")); Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); @@ -120,33 +127,5 @@ void loop() { motor.move(); // user communication - motor.command(serialReceiveUserCommand()); + command.run(); } - -// utility function enabling serial communication the user -String serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - String command = ""; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - - // end of user input - if (inChar == '\n') { - - // execute the user command - command = received_chars; - - // reset the command buffer - received_chars = ""; - } - } - return command; -} - diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index bcfb846a..2240d937 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -53,6 +53,11 @@ Encoder encoder = Encoder(2, 3, 8192); void doA(){encoder.handleA();} void doB(){encoder.handleB();} + +// commander interface +Commander command = Commander(Serial); +void onA(String cmd){ command.motor(&motor, cmd); } + void setup() { // initialize encoder sensor hardware @@ -103,6 +108,8 @@ void setup() { // set the inital target value motor.target = 2; + // define the motor id + command.add('A', onA); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); @@ -121,33 +128,5 @@ void loop() { motor.move(); // user communication - motor.command(serialReceiveUserCommand()); -} - -// utility function enabling serial communication the user -String serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - String command = ""; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - - // end of user input - if (inChar == '\n') { - - // execute the user command - command = received_chars; - - // reset the command buffer - received_chars = ""; - } - } - return command; -} - + command.run(); +}s \ No newline at end of file diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 3cec7de6..d1889770 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -59,6 +59,12 @@ void doC(){sensor.handleC();} // If no available hadware interrupt pins use the software interrupt PciListenerImp listenC(sensor.pinC, doC); + +// commander interface +Commander command = Commander(Serial); +void onA(String cmd){ command.motor(&motor, cmd); } + + void setup() { // initialize encoder sensor hardware @@ -110,6 +116,8 @@ void setup() { // set the inital target value motor.target = 2; + // define the motor id + command.add('A', onA); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); @@ -128,33 +136,7 @@ void loop() { motor.move(); // user communication - motor.command(serialReceiveUserCommand()); + command.run(); } -// utility function enabling serial communication the user -String serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - String command = ""; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - - // end of user input - if (inChar == '\n') { - - // execute the user command - command = received_chars; - - // reset the command buffer - received_chars = ""; - } - } - return command; -} diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 410ec48b..1f11e1fb 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -53,6 +53,10 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); +// commander interface +Commander command = Commander(Serial); +void onA(String cmd){ command.motor(&motor, cmd); } + void setup() { // initialise magnetic sensor hardware @@ -102,6 +106,8 @@ void setup() { // set the inital target value motor.target = 2; + // define the motor id + command.add('A', onA); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); @@ -120,33 +126,6 @@ void loop() { motor.move(); // user communication - motor.command(serialReceiveUserCommand()); -} - -// utility function enabling serial communication the user -String serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - String command = ""; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - - // end of user input - if (inChar == '\n') { - - // execute the user command - command = received_chars; - - // reset the command buffer - received_chars = ""; - } - } - return command; + command.run(); } diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index de13e7ae..7ffc54c5 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -63,6 +63,8 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8); BLDCMotor motor = BLDCMotor(11); BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); +// commander interface +Commander command = Commander(Serial); void setup() { Serial.begin(115200); @@ -126,7 +128,7 @@ void cmdControl(OSCMessage &msg){ if (msg.isString(0)) { msg.getString(0,cmdStr,16); String it(cmdStr); - motor.command(it); + command.motor(&motor,it); } } diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index a0b7b881..e11f3c85 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -103,9 +103,11 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // checks if driver phases are the same as current sense phases // and checks the direction of measuremnt. _delay(500); - if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); - + if(exit_flag){ + if(current_sense) exit_flag *= alignCurrentSense(); + else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); + } + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ From 1fe11b330c62535f0292d28ab68a73134c16f18f Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 17 Feb 2021 20:16:00 +0100 Subject: [PATCH 097/749] fix typo --- .../encoder/full_control_serial/full_control_serial.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 2240d937..7866e13f 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -129,4 +129,4 @@ void loop() { // user communication command.run(); -}s \ No newline at end of file +} \ No newline at end of file From ef337ee8d960cb69d029bbedf997356e2d9b85ff Mon Sep 17 00:00:00 2001 From: David Date: Wed, 17 Feb 2021 15:28:34 -0600 Subject: [PATCH 098/749] Example typo --- .../open_loop_position_example/open_loop_position_example.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 7e35441f..e1c4e31c 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -5,7 +5,7 @@ // BLDC motor & driver instance // BLDCMotor motor = BLDCMotor(pole pair number); BLDCMotor motor = BLDCMotor(11); -// BLDCMotor motor = BLDCMotor(pole pair number); +// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // Stepper motor & driver instance From 9712ead4677861a084e4a7e2f6a7f1516ec6256e Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 18 Feb 2021 08:55:33 +0100 Subject: [PATCH 099/749] FIX _isset error --- src/drivers/BLDCDriver6PWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index cd0d842b..013e6415 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -55,7 +55,7 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration - if( _isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; // configure 6pwm // hardware specific function - depending on driver and mcu From 98712b625fb9777c6336fc044ba0bb5b20f75bbb Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 18 Feb 2021 13:57:40 +0100 Subject: [PATCH 100/749] added monitoring control through commands + motion loop downsampling --- src/BLDCMotor.cpp | 3 + src/StepperMotor.cpp | 2 + src/common/base_classes/CommunicationNode.h | 15 -- src/common/base_classes/FOCMotor.cpp | 63 +++--- src/common/base_classes/FOCMotor.h | 9 +- src/common/defaults.h | 2 +- src/communication/Commander.cpp | 219 +++++++++++++------- src/communication/Commander.h | 14 +- src/communication/commands.h | 56 +++-- 9 files changed, 239 insertions(+), 144 deletions(-) delete mode 100644 src/common/base_classes/CommunicationNode.h diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index e11f3c85..ab38df38 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -289,6 +289,9 @@ void BLDCMotor::loopFOC() { void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; // get angular velocity diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index e5906e97..1c28fb1e 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -178,6 +178,8 @@ void StepperMotor::loopFOC() { void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; // set internal target variable if(_isset(new_target) ) target = new_target; // get angular velocity diff --git a/src/common/base_classes/CommunicationNode.h b/src/common/base_classes/CommunicationNode.h deleted file mode 100644 index a688ef48..00000000 --- a/src/common/base_classes/CommunicationNode.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef COMNODE_h -#define COMNODE_h - -#include "Arduino.h" - -/** - * Interface class for communication - */ -class CommunicationNode{ - public: - virtual String communicate(String use_command)=0; -}; - - -#endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 193a7ad1..b9e4a479 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -81,31 +81,46 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { + if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return; + monitor_cnt = 0; if(!monitor_port) return; - switch (controller) { - case MotionControlType::velocity_openloop: - case MotionControlType::velocity: - monitor_port->print(voltage.q); - monitor_port->print("\t"); - monitor_port->print(shaft_velocity_sp); - monitor_port->print("\t"); - monitor_port->println(shaft_velocity); - break; - case MotionControlType::angle_openloop: - case MotionControlType::angle: - monitor_port->print(voltage.q); - monitor_port->print("\t"); - monitor_port->print(shaft_angle_sp); - monitor_port->print("\t"); - monitor_port->println(shaft_angle); - break; - case MotionControlType::torque: - monitor_port->print(voltage.q); - monitor_port->print("\t"); - monitor_port->print(shaft_angle); - monitor_port->print("\t"); - monitor_port->println(shaft_velocity); - break; + bool printed = 0; + + if(monitor_variables[0]){ + monitor_port->print(target); + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables[1]) { + monitor_port->print(voltage.q); + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables[2]) { + monitor_port->print(voltage.d); + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables[3]) { + monitor_port->print(current.q*1000); // mAmps + monitor_port->print("\t"); + printed= true; } + if(monitor_variables[4]) { + monitor_port->print(current.d*1000); // mAmps + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables[5]) { + monitor_port->print(shaft_velocity); + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables[6]) { + monitor_port->print(shaft_angle); + printed= true; + } + if(printed) monitor_port->println(); + } diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 603ec024..c761c164 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -142,11 +142,11 @@ class FOCMotor float velocity_limit; //!< Velocity limitting variable - global limit // motor status vairables - int enabled = 0;//!< enabled or disabled motor flag + int8_t enabled = 0;//!< enabled or disabled motor flag // pwm modulation related variables FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm - int modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2 or (0) pulled to 0 + int8_t modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2 or (0) pulled to 0 // configuration structures @@ -162,6 +162,8 @@ class FOCMotor PIDController P_angle{DEF_P_ANGLE_P,0,0,1e10,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 = 0; //!< parameter defining the ratio of downsampling for move commad + unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad // sensor related variabels float sensor_offset; //!< user defined sensor zero offset @@ -181,6 +183,9 @@ class FOCMotor * significantly slowing the execution down!!!! */ void monitor(); + unsigned int monitor_cnt = 0 ; + unsigned int monitor_downsample = 1; + bool monitor_variables[7] = {0}; /** * Sensor link: diff --git a/src/common/defaults.h b/src/common/defaults.h index 80105598..fa1ea6ad 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -25,7 +25,7 @@ #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_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.001 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.002 //!< default currnet filter time constant #endif // default current limit values #define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index a968fbbe..2028f207 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -15,59 +15,69 @@ void Commander::run(){ // a string to hold incoming data while (com_port->available()) { // get the new byte: - char inChar = (char)com_port->read(); - // add it to the string buffer: - received_chars += inChar; + received_chars[rec_cnt] = (char)com_port->read(); // end of user input - if (inChar == '\n') { + if (received_chars[rec_cnt++] == '\n') { // execute the user command - char id = received_chars.charAt(0); - for(int i=0; i < call_count; i++){ - if(id == call_ids[i]){ - call_list[i](received_chars.substring(1)); - break; + char id = received_chars[0]; + if(id == CMD_SCAN) + for(int i=0; i < call_count; i++){ + com_port->print(call_ids[i]); + com_port->print(":"); + call_list[i](cmd_scan_msg); } - } + else + for(int i=0; i < call_count; i++){ + if(id == call_ids[i]){ + call_list[i](&received_chars[1]); + break; + } + } + // reset the command buffer - received_chars = ""; + received_chars[0] = 0; + rec_cnt=0; } } } -void Commander::motor(FOCMotor* motor, String user_command) { +void Commander::motor(FOCMotor* motor, char* user_command) { // if empty string - if( user_command.length() < 1 ) return; + if( user_command[0] == CMD_SCAN ){ + com_port->println(F("mot")); + return; + } // parse command letter - char cmd = user_command.charAt(0); - char sub_cmd = user_command.charAt(1); + char cmd = user_command[0]; + 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.charAt(value_index) == '\n'; + bool GET = user_command[value_index] == '\n'; // parse command values - float value = user_command.substring(value_index).toFloat(); + float value = atof(&user_command[value_index]); // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ case CMD_C_Q_PID: // com_port->print(F("PID curr q| ")); - if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, user_command.substring(1)); - else pid(&motor->PID_current_q,user_command.substring(1)); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]); + else pid(&motor->PID_current_q,&user_command[1]); break; case CMD_C_D_PID: // com_port->print(F("PID curr d| ")); - if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, user_command.substring(1)); - else pid(&motor->PID_current_d, user_command.substring(1)); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]); + else pid(&motor->PID_current_d, &user_command[1]); break; case CMD_V_PID: // com_port->print(F("PID vel| ")); - if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, user_command.substring(1)); - else pid(&motor->PID_velocity, user_command.substring(1)); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]); + else pid(&motor->PID_velocity, &user_command[1]); break; case CMD_A_PID: // com_port->print(F("PID angle| ")); - if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, user_command.substring(1)); - else pid(&motor->P_angle, user_command.substring(1)); + if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]); + else pid(&motor->P_angle, &user_command[1]); break; case CMD_LIMITS: // com_port->print(F("Limits| ")); @@ -109,26 +119,35 @@ void Commander::motor(FOCMotor* motor, String user_command) { break; case CMD_MOTION_TYPE: com_port->print(F("Motion: ")); - // change control type - if(!GET && value >= 0 && (int)value < 5)// if set command - motor->controller = (MotionControlType)value; - switch(motor->controller){ - case MotionControlType::torque: - com_port->println(F("torque")); - break; - case MotionControlType::velocity: - com_port->println(F("vel")); + switch(sub_cmd){ + case SCMD_DOWNSAMPLE: + com_port->print(F("downsample: ")); + if(!GET) motor->motion_downsample = value; + com_port->println(motor->motion_downsample); break; - case MotionControlType::angle: - com_port->println(F("angle")); - break; - case MotionControlType::velocity_openloop: - com_port->println(F("vel open")); - break; - case MotionControlType::angle_openloop: - com_port->println(F("angle open")); - break; - } + default: + // change control type + if(!GET && value >= 0 && (int)value < 5)// if set command + motor->controller = (MotionControlType)value; + switch(motor->controller){ + case MotionControlType::torque: + com_port->println(F("torque")); + break; + case MotionControlType::velocity: + com_port->println(F("vel")); + break; + case MotionControlType::angle: + com_port->println(F("angle")); + break; + case MotionControlType::velocity_openloop: + com_port->println(F("vel open")); + break; + case MotionControlType::angle_openloop: + com_port->println(F("angle open")); + break; + } + break; + } break; case CMD_TORQUE_TYPE: // change control type @@ -185,36 +204,80 @@ void Commander::motor(FOCMotor* motor, String user_command) { } break; case CMD_MONITOR: // get current values of the state variables - switch((uint8_t)value){ - case 0: // get voltage - com_port->print(F("Uq: ")); - com_port->println(motor->voltage.q); - break; - case 1: // get velocity - com_port->print(F("vel: ")); - com_port->println(motor->shaft_velocity); - break; - case 2: // get angle - com_port->print(F("Angle: ")); - com_port->println(motor->shaft_angle); - break; - case 3: // get angle - com_port->print(F("target: ")); - com_port->println(motor->target); - break; - } + com_port->print(F("Monitor | ")); + switch (sub_cmd){ + case SCMD_GET: // get command + switch((uint8_t)value){ + case 0: // get target + com_port->print(F("target: ")); + com_port->println(motor->target); + break; + case 1: // get voltage q + com_port->print(F("Vq: ")); + com_port->println(motor->voltage.q); + break; + case 2: // get voltage d + com_port->print(F("Vd: ")); + com_port->println(motor->voltage.q); + break; + case 3: // get current q + com_port->print(F("Cq: ")); + com_port->println(motor->voltage.q); + break; + case 4: // get current d + com_port->print(F("Cd: ")); + com_port->println(motor->voltage.q); + break; + case 5: // get velocity + com_port->print(F("vel: ")); + com_port->println(motor->shaft_velocity); + break; + case 6: // get angle + com_port->print(F("Angle: ")); + com_port->println(motor->shaft_angle); + break; + default: + com_port->println(F("err")); + break; + } + break; + case SCMD_DOWNSAMPLE: + com_port->print(F("downsample: ")); + if(!GET) motor->monitor_downsample = value; + com_port->println(motor->monitor_downsample); + break; + case SCMD_CLEAR: + for(int i=0; i<7; i++) motor->monitor_variables[i] = 0; + com_port->println(F("clear")); + break; + case SCMD_SET: + for(int i=0; i<7; i++){ + motor->monitor_variables[i] = user_command[value_index+i] - '0'; + com_port->print(motor->monitor_variables[i]); + } + com_port->println(); + break; + default: + com_port->println(F("err")); + break; + } + break; default: // target change com_port->print(F("Target: ")); - motor->target = user_command.toFloat(); + motor->target = atof(user_command); com_port->println(motor->target); } } -void Commander::pid(PIDController* pid, String user_cmd){ - char cmd = user_cmd.charAt(0); - bool GET = user_cmd.charAt(1) == '\n'; - float value = user_cmd.substring(1).toFloat(); +void Commander::pid(PIDController* pid, char* user_cmd){ + if( user_cmd[0] == CMD_SCAN ){ + com_port->println(F("pid")); + return; + } + char cmd = user_cmd[0]; + bool GET = user_cmd[1] == '\n'; + float value = atof(&user_cmd[1]); switch (cmd){ case SCMD_PID_P: // P gain change @@ -248,10 +311,14 @@ void Commander::pid(PIDController* pid, String user_cmd){ } } -void Commander::lpf(LowPassFilter* lpf, String user_cmd){ - char cmd = user_cmd.charAt(0); - bool GET = user_cmd.charAt(1) == '\n'; - float value = user_cmd.substring(1).toFloat(); +void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ + if( user_cmd[0] == CMD_SCAN ){ + com_port->println(F("lpf")); + return; + } + char cmd = user_cmd[0]; + bool GET = user_cmd[1] == '\n'; + float value = atof(&user_cmd[1]); switch (cmd){ case SCMD_LPF_TF: // Tf value change @@ -265,8 +332,12 @@ void Commander::lpf(LowPassFilter* lpf, String user_cmd){ } } -void Commander::scalar(float* value, String user_cmd){ - bool GET = user_cmd.charAt(0) == '\n'; - if(!GET) *value = user_cmd.toFloat(); +void Commander::variable(float* value, char* user_cmd){ + if( user_cmd[0] == CMD_SCAN ){ + com_port->println(F("var")); + return; + } + bool GET = user_cmd[0] == '\n'; + if(!GET) *value = atof(user_cmd); com_port->println(*value); } \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 30d5edd0..b44ed7be 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -8,7 +8,7 @@ #include "commands.h" -typedef void (* CommandCallback)(String); +typedef void (* CommandCallback)(char*); class Commander { @@ -68,16 +68,18 @@ class Commander * * returns 0 for error or 1 for executed command */ - void motor(FOCMotor* motor, String user_cmd); - void lpf(LowPassFilter* lpf, String user_cmd); - void pid(PIDController* pid, String user_cmd); - void scalar(float* value, String user_cmd); + void motor(FOCMotor* motor, char* user_cmd); + void lpf(LowPassFilter* lpf, char* user_cmd); + void pid(PIDController* pid, char* user_cmd); + void variable(float* value, char* user_cmd); private: CommandCallback call_list[20]; char call_ids[20]; //!< Dictionary of nodes int call_count = 0; - String received_chars = ""; + char received_chars[20] = {0}; + int rec_cnt = 0; + char cmd_scan_msg[2] = {CMD_SCAN,0}; }; diff --git a/src/communication/commands.h b/src/communication/commands.h index 7c7969b4..bd4288f9 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -1,30 +1,42 @@ #ifndef COMMANDS_h #define COMMANDS_h +// see docs.simplefoc.com for in depth explanation of the commands + // list o commands - #define CMD_C_D_PID 'D' - #define CMD_C_Q_PID 'Q' - #define CMD_V_PID 'V' - #define CMD_A_PID 'A' - #define CMD_STATUS 'E' - #define CMD_LIMITS 'L' - #define CMD_MOTION_TYPE 'C' - #define CMD_TORQUE_TYPE 'T' - #define CMD_SENSOR 'S' - #define CMD_MONITOR 'M' - #define CMD_RESIST 'R' + #define CMD_C_D_PID 'D' //!< current d PID & LPF + #define CMD_C_Q_PID 'Q' //!< current d PID & LPF + #define CMD_V_PID 'V' //!< velocity PID & LPF + #define CMD_A_PID 'A' //!< angle PID & LPF + #define CMD_STATUS 'E' //!< motor status enable/disable + #define CMD_LIMITS 'L' //!< limits current/voltage/velocity + #define CMD_MOTION_TYPE 'C' //!< motion control type + #define CMD_TORQUE_TYPE 'T' //!< torque control type + #define CMD_SENSOR 'S' //!< sensor offsets + #define CMD_MONITOR 'M' //!< monitoring + #define CMD_RESIST 'R' //!< motor phase resistance + + #define CMD_SCAN '?' //!< command scaning the network - only for commander // subcomands - #define SCMD_PID_P 'P' - #define SCMD_PID_I 'I' - #define SCMD_PID_D 'D' - #define SCMD_PID_RAMP 'R' - #define SCMD_PID_LIM 'L' - #define SCMD_LPF_TF 'F' - #define SCMD_LIM_CURR 'C' - #define SCMD_LIM_VOLT 'U' - #define SCMD_LIM_VEL 'V' - #define SCMD_SENS_MECH_OFFSET 'M' - #define SCMD_SENS_ELEC_OFFSET 'E' + //pid - lpf + #define SCMD_PID_P 'P' //!< PID gain P + #define SCMD_PID_I 'I' //!< PID gain I + #define SCMD_PID_D 'D' //!< PID gain D + #define SCMD_PID_RAMP 'R' //!< PID ramp + #define SCMD_PID_LIM 'L' //!< PID limit + #define SCMD_LPF_TF 'F' //!< LPF time constant + // limits + #define SCMD_LIM_CURR 'C' //!< Limit current + #define SCMD_LIM_VOLT 'U' //!< Limit voltage + #define SCMD_LIM_VEL 'V' //!< Limit velocity + //sensor + #define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset + #define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset + // monitoring + #define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value + #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 #endif \ No newline at end of file From 68745080bb946fba89a06af112ee7eea1360a092 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 18 Feb 2021 14:04:19 +0100 Subject: [PATCH 101/749] examples changed --- .../encoder/full_control_serial/full_control_serial.ino | 2 +- .../encoder/full_control_serial/full_control_serial.ino | 2 +- .../encoder/full_control_serial/full_control_serial.ino | 2 +- .../hall_sensor/full_control_serial/full_control_serial.ino | 2 +- .../magnetic_sensor/full_control_serial/full_control_serial.ino | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 3e1f3357..c9fff7c5 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -39,7 +39,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(String cmd){ command.motor(&motor, cmd); } +void onA(char* cmd){ command.motor(&motor, cmd); } void setup() { diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 2ac51d20..50962208 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -42,7 +42,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(String cmd){ command.motor(&motor, cmd); } +void onA(char* cmd){ command.motor(&motor, cmd); } void setup() { diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 7866e13f..5c68b29e 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -56,7 +56,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(String cmd){ command.motor(&motor, cmd); } +void onA(char* cmd){ command.motor(&motor, cmd); } void setup() { diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index d1889770..df62b28e 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -62,7 +62,7 @@ PciListenerImp listenC(sensor.pinC, doC); // commander interface Commander command = Commander(Serial); -void onA(String cmd){ command.motor(&motor, cmd); } +void onA(char* cmd){ command.motor(&motor, cmd); } void setup() { diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 1f11e1fb..5b7edc52 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -55,7 +55,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // commander interface Commander command = Commander(Serial); -void onA(String cmd){ command.motor(&motor, cmd); } +void onA(char* cmd){ command.motor(&motor, cmd); } void setup() { From 39cf92e69a81dbe24523148dd5aa09e470027b09 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 18 Feb 2021 18:55:45 +0100 Subject: [PATCH 102/749] FEAY added support: verbose commands + decimal places + string input --- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 3 +- keywords.txt | 3 + src/communication/Commander.cpp | 156 ++++++++++-------- src/communication/Commander.h | 51 +----- 4 files changed, 98 insertions(+), 115 deletions(-) diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index 7ffc54c5..0ebfc51a 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -127,8 +127,7 @@ void cmdControl(OSCMessage &msg){ char cmdStr[16]; if (msg.isString(0)) { msg.getString(0,cmdStr,16); - String it(cmdStr); - command.motor(&motor,it); + command.motor(&motor,cmdStr); } } diff --git a/keywords.txt b/keywords.txt index 2eab2f69..774a5c57 100644 --- a/keywords.txt +++ b/keywords.txt @@ -107,6 +107,9 @@ skip_align KEYWORD2 sensor_direction KEYWORD2 sensor_offset KEYWORD2 zero_electric_angle KEYWORD2 +verbose KEYWORD2 +decimal_places KEYWORD2 + voltage KEYWORD2 diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 2028f207..74227a0e 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -19,20 +19,7 @@ void Commander::run(){ // end of user input if (received_chars[rec_cnt++] == '\n') { // execute the user command - char id = received_chars[0]; - if(id == CMD_SCAN) - for(int i=0; i < call_count; i++){ - com_port->print(call_ids[i]); - com_port->print(":"); - call_list[i](cmd_scan_msg); - } - else - for(int i=0; i < call_count; i++){ - if(id == call_ids[i]){ - call_list[i](&received_chars[1]); - break; - } - } + run(received_chars); // reset the command buffer received_chars[0] = 0; @@ -41,6 +28,35 @@ void Commander::run(){ } } +void Commander::run(char* user_input){ + // execute the user command + char id = received_chars[0]; + if(id == CMD_SCAN) + for(int i=0; i < call_count; i++){ + com_port->print(call_ids[i]); + com_port->print(":"); + call_list[i](cmd_scan_msg); + } + else + for(int i=0; i < call_count; i++){ + if(id == call_ids[i]){ + call_list[i](&received_chars[1]); + break; + } + } +} + +void Commander::verbosePrint(const char* message){ + if(verbose) com_port->print(message); +} +void Commander::verbosePrint(const __FlashStringHelper *message){ + if(verbose) com_port->print(message); +} +void Commander::printNumber(const float number, const bool newline){ + if(newline) com_port->println(number,decimal_places); + else com_port->print(number,decimal_places); +} + void Commander::motor(FOCMotor* motor, char* user_command) { // if empty string if( user_command[0] == CMD_SCAN ){ @@ -60,30 +76,30 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ case CMD_C_Q_PID: // - com_port->print(F("PID curr q| ")); + verbosePrint(F("PID curr q| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]); else pid(&motor->PID_current_q,&user_command[1]); break; case CMD_C_D_PID: // - com_port->print(F("PID curr d| ")); + verbosePrint(F("PID curr d| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]); else pid(&motor->PID_current_d, &user_command[1]); break; case CMD_V_PID: // - com_port->print(F("PID vel| ")); + verbosePrint(F("PID vel| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]); else pid(&motor->PID_velocity, &user_command[1]); break; case CMD_A_PID: // - com_port->print(F("PID angle| ")); + verbosePrint(F("PID angle| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]); else pid(&motor->P_angle, &user_command[1]); break; case CMD_LIMITS: // - com_port->print(F("Limits| ")); + verbosePrint(F("Limits| ")); switch (sub_cmd){ case SCMD_LIM_VOLT: // voltage limit change - com_port->print(F("volt: ")); + verbosePrint(F("volt: ")); if(!GET) { motor->voltage_limit = value; motor->PID_current_d.limit = value; @@ -91,10 +107,10 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // change velocity pid limit if in voltage mode and no phase resistance set if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value; } - com_port->println(motor->voltage_limit); + printNumber(motor->voltage_limit,1); break; case SCMD_LIM_CURR: // current limit - com_port->print(F("curr: ")); + verbosePrint(F("curr: ")); if(!GET){ motor->current_limit = value; // if phase resistance is set, change the voltage limit as well. @@ -102,15 +118,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // 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; } - com_port->println(motor->current_limit); + printNumber(motor->current_limit,1); break; case SCMD_LIM_VEL: // velocity limit - com_port->print(F("vel: ")); + verbosePrint(F("vel: ")); if(!GET){ motor->velocity_limit = value; motor->P_angle.limit = value; } - com_port->println(motor->velocity_limit); + printNumber(motor->velocity_limit,1); break; default: com_port->println(F("err")); @@ -118,12 +134,12 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MOTION_TYPE: - com_port->print(F("Motion: ")); + verbosePrint(F("Motion: ")); switch(sub_cmd){ case SCMD_DOWNSAMPLE: - com_port->print(F("downsample: ")); + verbosePrint(F("downsample: ")); if(!GET) motor->motion_downsample = value; - com_port->println(motor->motion_downsample); + printNumber(motor->motion_downsample,1); break; default: // change control type @@ -151,7 +167,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case CMD_TORQUE_TYPE: // change control type - com_port->print(F("Torque: ")); + verbosePrint(F("Torque: ")); if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ @@ -168,13 +184,13 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case CMD_STATUS: // enable/disable - com_port->print(F("Status: ")); + verbosePrint(F("Status: ")); if(!GET) (bool)value ? motor->enable() : motor->disable(); com_port->println(motor->enabled); break; case CMD_RESIST: // enable/disable - com_port->print(F("R phase: ")); + verbosePrint(F("R phase: ")); if(!GET){ motor->phase_resistance = value; if(motor->torque_controller==TorqueControlType::voltage){ @@ -182,21 +198,22 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->PID_velocity.limit= motor->current_limit; } } - com_port->println(_isset(motor->phase_resistance) ? motor->phase_resistance : 0 ); + if(_isset(motor->phase_resistance)) printNumber(motor->phase_resistance,1); + else com_port->println(0); break; case CMD_SENSOR: // Sensor zero offset - com_port->print(F("Sensor | ")); + verbosePrint(F("Sensor | ")); switch (sub_cmd){ case SCMD_SENS_MECH_OFFSET: // zero offset - com_port->print(F("offset: ")); + verbosePrint(F("offset: ")); if(!GET) motor->sensor_offset = value; - com_port->println(motor->sensor_offset); + printNumber(motor->sensor_offset,1); break; case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch - com_port->print(F("el. offset: ")); + verbosePrint(F("el. offset: ")); if(!GET) motor->zero_electric_angle = value; - com_port->println(motor->zero_electric_angle); + printNumber(motor->zero_electric_angle,1); break; default: com_port->println(F("err")); @@ -204,37 +221,37 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MONITOR: // get current values of the state variables - com_port->print(F("Monitor | ")); + verbosePrint(F("Monitor | ")); switch (sub_cmd){ case SCMD_GET: // get command switch((uint8_t)value){ case 0: // get target - com_port->print(F("target: ")); - com_port->println(motor->target); + verbosePrint(F("target: ")); + printNumber(motor->target,1); break; case 1: // get voltage q - com_port->print(F("Vq: ")); - com_port->println(motor->voltage.q); + verbosePrint(F("Vq: ")); + printNumber(motor->voltage.q,1); break; case 2: // get voltage d - com_port->print(F("Vd: ")); - com_port->println(motor->voltage.q); + verbosePrint(F("Vd: ")); + printNumber(motor->voltage.q,1); break; case 3: // get current q - com_port->print(F("Cq: ")); - com_port->println(motor->voltage.q); + verbosePrint(F("Cq: ")); + printNumber(motor->voltage.q,1); break; case 4: // get current d - com_port->print(F("Cd: ")); - com_port->println(motor->voltage.q); + verbosePrint(F("Cd: ")); + printNumber(motor->voltage.q,1); break; case 5: // get velocity - com_port->print(F("vel: ")); - com_port->println(motor->shaft_velocity); + verbosePrint(F("vel: ")); + printNumber(motor->shaft_velocity,1); break; case 6: // get angle - com_port->print(F("Angle: ")); - com_port->println(motor->shaft_angle); + verbosePrint(F("Angle: ")); + printNumber(motor->shaft_angle,1); break; default: com_port->println(F("err")); @@ -242,9 +259,9 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case SCMD_DOWNSAMPLE: - com_port->print(F("downsample: ")); + verbosePrint(F("downsample: ")); if(!GET) motor->monitor_downsample = value; - com_port->println(motor->monitor_downsample); + printNumber(motor->monitor_downsample,1); break; case SCMD_CLEAR: for(int i=0; i<7; i++) motor->monitor_variables[i] = 0; @@ -261,12 +278,11 @@ void Commander::motor(FOCMotor* motor, char* user_command) { com_port->println(F("err")); break; } - break; default: // target change - com_port->print(F("Target: ")); + verbosePrint(F("Target: ")); motor->target = atof(user_command); - com_port->println(motor->target); + printNumber(motor->target,1); } } @@ -281,29 +297,29 @@ void Commander::pid(PIDController* pid, char* user_cmd){ switch (cmd){ case SCMD_PID_P: // P gain change - com_port->print("P: "); + verbosePrint("P: "); if(!GET) pid->P = value; - com_port->println(pid->P); + printNumber(pid->P,1); break; case SCMD_PID_I: // I gain change - com_port->print("I: "); + verbosePrint("I: "); if(!GET) pid->I = value; - com_port->println(pid->I); + printNumber(pid->I,1); break; case SCMD_PID_D: // D gain change - com_port->print("D: "); + verbosePrint("D: "); if(!GET) pid->D = value; - com_port->println(pid->D); + printNumber(pid->D,1); break; case SCMD_PID_RAMP: // ramp change - com_port->print("ramp: "); + verbosePrint("ramp: "); if(!GET) pid->output_ramp = value; - com_port->println(pid->output_ramp); + printNumber(pid->output_ramp,1); break; case SCMD_PID_LIM: // limit change - com_port->print("limit: "); + verbosePrint("limit: "); if(!GET) pid->limit = value; - com_port->println(pid->limit); + printNumber(pid->limit,1); break; default: com_port->println(F("err")); @@ -322,9 +338,9 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ switch (cmd){ case SCMD_LPF_TF: // Tf value change - com_port->print("Tf: "); + verbosePrint("Tf: "); if(!GET) lpf->Tf = value; - com_port->println(lpf->Tf); + printNumber(lpf->Tf,1); break; default: com_port->println(F("err")); @@ -339,5 +355,5 @@ void Commander::variable(float* value, char* user_cmd){ } bool GET = user_cmd[0] == '\n'; if(!GET) *value = atof(user_cmd); - com_port->println(*value); + printNumber(*value,1); } \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h index b44ed7be..c0bfd01e 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -19,55 +19,16 @@ class Commander Commander(HardwareSerial &serial); void run(); + void run(char* user_input); void add(char id , CommandCallback onCommand); + bool verbose = 1; + uint8_t decimal_places = 3; + // monitoring functions HardwareSerial* com_port; //!< Serial terminal variable if provided - /** - * Function setting the configuration parameters of the motor, target value of the control loop - * and outputing them to the monitoring port( if available ) : - * - configure PID controller constants - * - change motion control loops - * - monitor motor variabels - * - set target values - * - check all the configuration values - * - * To check the config value just enter the command letter. - * For example: - * - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - * - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PI controller P gain - * - I: velocity PI controller I gain - * - L: velocity PI controller voltage limit - * - R: velocity PI controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: angle - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * - * - Look into the documentation (docs.simplefoc.com) for more information. - * - * @param command String containing the user command - * - * returns 0 for error or 1 for executed command - */ void motor(FOCMotor* motor, char* user_cmd); void lpf(LowPassFilter* lpf, char* user_cmd); void pid(PIDController* pid, char* user_cmd); @@ -80,6 +41,10 @@ class Commander char received_chars[20] = {0}; int rec_cnt = 0; char cmd_scan_msg[2] = {CMD_SCAN,0}; + + void verbosePrint(const char* message); + void verbosePrint(const __FlashStringHelper *message); + void printNumber(const float number, const bool newline = 0); }; From 27f5215f35ec7ac11da1b5b20db196143af09c96 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 18 Feb 2021 21:38:15 +0100 Subject: [PATCH 103/749] commander refactor allow no serial --- src/communication/Commander.cpp | 174 ++++++++++++++++++++------------ src/communication/Commander.h | 118 +++++++++++++++++++--- 2 files changed, 215 insertions(+), 77 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 74227a0e..9c66556f 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -4,6 +4,10 @@ Commander::Commander(HardwareSerial& serial){ com_port = &serial; } +Commander::Commander(){ + // do nothing +} + void Commander::add(char id, CommandCallback onCommand){ call_list[call_count] = onCommand; @@ -11,7 +15,9 @@ void Commander::add(char id, CommandCallback onCommand){ call_count++; } + void Commander::run(){ + if(!com_port) return; // a string to hold incoming data while (com_port->available()) { // get the new byte: @@ -28,39 +34,45 @@ void Commander::run(){ } } +void Commander::run(HardwareSerial &serial){ + // a string to hold incoming data + while (serial.available()) { + // get the new byte: + received_chars[rec_cnt] = (char)serial.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; + } + } +} + void Commander::run(char* user_input){ // execute the user command - char id = received_chars[0]; + char id = user_input[0]; if(id == CMD_SCAN) for(int i=0; i < call_count; i++){ - com_port->print(call_ids[i]); - com_port->print(":"); + print(call_ids[i], 0); + print(":", 0); call_list[i](cmd_scan_msg); } else for(int i=0; i < call_count; i++){ if(id == call_ids[i]){ - call_list[i](&received_chars[1]); + call_list[i](&user_input[1]); break; } } } -void Commander::verbosePrint(const char* message){ - if(verbose) com_port->print(message); -} -void Commander::verbosePrint(const __FlashStringHelper *message){ - if(verbose) com_port->print(message); -} -void Commander::printNumber(const float number, const bool newline){ - if(newline) com_port->println(number,decimal_places); - else com_port->print(number,decimal_places); -} - void Commander::motor(FOCMotor* motor, char* user_command) { // if empty string if( user_command[0] == CMD_SCAN ){ - com_port->println(F("mot")); + print(F("mot"),1); return; } @@ -107,7 +119,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // change velocity pid limit if in voltage mode and no phase resistance set if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value; } - printNumber(motor->voltage_limit,1); + print(motor->voltage_limit,1); break; case SCMD_LIM_CURR: // current limit verbosePrint(F("curr: ")); @@ -118,7 +130,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // 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; } - printNumber(motor->current_limit,1); + print(motor->current_limit,1); break; case SCMD_LIM_VEL: // velocity limit verbosePrint(F("vel: ")); @@ -126,10 +138,10 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->velocity_limit = value; motor->P_angle.limit = value; } - printNumber(motor->velocity_limit,1); + print(motor->velocity_limit,1); break; default: - com_port->println(F("err")); + printError(); break; } break; @@ -139,7 +151,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_DOWNSAMPLE: verbosePrint(F("downsample: ")); if(!GET) motor->motion_downsample = value; - printNumber(motor->motion_downsample,1); + print((int)motor->motion_downsample, 1); break; default: // change control type @@ -147,19 +159,19 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->controller = (MotionControlType)value; switch(motor->controller){ case MotionControlType::torque: - com_port->println(F("torque")); + print(F("torque"),1); break; case MotionControlType::velocity: - com_port->println(F("vel")); + print(F("vel"),1); break; case MotionControlType::angle: - com_port->println(F("angle")); + print(F("angle"),1); break; case MotionControlType::velocity_openloop: - com_port->println(F("vel open")); + print(F("vel open"),1); break; case MotionControlType::angle_openloop: - com_port->println(F("angle open")); + print(F("angle open"),1); break; } break; @@ -172,13 +184,13 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ case TorqueControlType::voltage: - com_port->println(F("volt")); + print(F("volt"),1); break; case TorqueControlType::current: - com_port->println(F("curr")); + print(F("curr"),1); break; case TorqueControlType::foc_current: - com_port->println(F("foc")); + print(F("foc"),1); break; } break; @@ -186,7 +198,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // enable/disable verbosePrint(F("Status: ")); if(!GET) (bool)value ? motor->enable() : motor->disable(); - com_port->println(motor->enabled); + print(motor->enabled,1); break; case CMD_RESIST: // enable/disable @@ -198,8 +210,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->PID_velocity.limit= motor->current_limit; } } - if(_isset(motor->phase_resistance)) printNumber(motor->phase_resistance,1); - else com_port->println(0); + if(_isset(motor->phase_resistance)) print(motor->phase_resistance,1); + else print(0,0); break; case CMD_SENSOR: // Sensor zero offset @@ -208,15 +220,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_SENS_MECH_OFFSET: // zero offset verbosePrint(F("offset: ")); if(!GET) motor->sensor_offset = value; - printNumber(motor->sensor_offset,1); + print(motor->sensor_offset,1); break; case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch verbosePrint(F("el. offset: ")); if(!GET) motor->zero_electric_angle = value; - printNumber(motor->zero_electric_angle,1); + print(motor->zero_electric_angle,1); break; default: - com_port->println(F("err")); + printError(); break; } break; @@ -227,68 +239,68 @@ void Commander::motor(FOCMotor* motor, char* user_command) { switch((uint8_t)value){ case 0: // get target verbosePrint(F("target: ")); - printNumber(motor->target,1); + print(motor->target,1); break; case 1: // get voltage q verbosePrint(F("Vq: ")); - printNumber(motor->voltage.q,1); + print(motor->voltage.q,1); break; case 2: // get voltage d verbosePrint(F("Vd: ")); - printNumber(motor->voltage.q,1); + print(motor->voltage.q,1); break; case 3: // get current q verbosePrint(F("Cq: ")); - printNumber(motor->voltage.q,1); + print(motor->voltage.q,1); break; case 4: // get current d verbosePrint(F("Cd: ")); - printNumber(motor->voltage.q,1); + print(motor->voltage.q,1); break; case 5: // get velocity verbosePrint(F("vel: ")); - printNumber(motor->shaft_velocity,1); + print(motor->shaft_velocity,1); break; case 6: // get angle verbosePrint(F("Angle: ")); - printNumber(motor->shaft_angle,1); + print(motor->shaft_angle,1); break; default: - com_port->println(F("err")); + printError(); break; } break; case SCMD_DOWNSAMPLE: verbosePrint(F("downsample: ")); if(!GET) motor->monitor_downsample = value; - printNumber(motor->monitor_downsample,1); + print((int)motor->monitor_downsample,1); break; case SCMD_CLEAR: for(int i=0; i<7; i++) motor->monitor_variables[i] = 0; - com_port->println(F("clear")); + print(F("clear"),1); break; case SCMD_SET: for(int i=0; i<7; i++){ motor->monitor_variables[i] = user_command[value_index+i] - '0'; - com_port->print(motor->monitor_variables[i]); + print(motor->monitor_variables[i],0); } - com_port->println(); + print("",1); break; default: - com_port->println(F("err")); + printError(); break; } break; default: // target change verbosePrint(F("Target: ")); motor->target = atof(user_command); - printNumber(motor->target,1); + print(motor->target,1); } } void Commander::pid(PIDController* pid, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - com_port->println(F("pid")); + print(F("pid"),1); return; } char cmd = user_cmd[0]; @@ -299,37 +311,37 @@ void Commander::pid(PIDController* pid, char* user_cmd){ case SCMD_PID_P: // P gain change verbosePrint("P: "); if(!GET) pid->P = value; - printNumber(pid->P,1); + print(pid->P,1); break; case SCMD_PID_I: // I gain change verbosePrint("I: "); if(!GET) pid->I = value; - printNumber(pid->I,1); + print(pid->I,1); break; case SCMD_PID_D: // D gain change verbosePrint("D: "); if(!GET) pid->D = value; - printNumber(pid->D,1); + print(pid->D,1); break; case SCMD_PID_RAMP: // ramp change verbosePrint("ramp: "); if(!GET) pid->output_ramp = value; - printNumber(pid->output_ramp,1); + print(pid->output_ramp,1); break; case SCMD_PID_LIM: // limit change verbosePrint("limit: "); if(!GET) pid->limit = value; - printNumber(pid->limit,1); + print(pid->limit,1); break; default: - com_port->println(F("err")); + printError(); break; } } void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - com_port->println(F("lpf")); + print(F("lpf"),1); return; } char cmd = user_cmd[0]; @@ -338,22 +350,60 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ switch (cmd){ case SCMD_LPF_TF: // Tf value change - verbosePrint("Tf: "); + verbosePrint(F("Tf: ")); if(!GET) lpf->Tf = value; - printNumber(lpf->Tf,1); + print(lpf->Tf,1); break; default: - com_port->println(F("err")); + printError(); break; } } void Commander::variable(float* value, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - com_port->println(F("var")); + print(F("var"),1); return; } bool GET = user_cmd[0] == '\n'; if(!GET) *value = atof(user_cmd); - printNumber(*value,1); + print(*value,1); +} + + +void Commander::print(const int number, const bool newline){ + if(!com_port) return; + if(newline) com_port->println(number); + else com_port->print(number); +} +void Commander::print(const float number, const bool newline){ + if(!com_port) return; + if(newline) com_port->println(number,decimal_places); + else com_port->print(number,decimal_places); +} +void Commander::print(const char* message, const bool newline){ + if(!com_port) return; + if(newline) com_port->println(message); + else com_port->print(message); +} +void Commander::print(const __FlashStringHelper *message, const bool newline){ + if(!com_port) return; + if(newline) com_port->println(message); + else com_port->print(message); +} +void Commander::print(const char message, const bool newline){ + if(!com_port) return; + if(newline) com_port->println(message); + else com_port->print(message); +} + + +void Commander::verbosePrint(const char* message){ + if(verbose) print(message,0); +} +void Commander::verbosePrint(const __FlashStringHelper *message){ + if(verbose) print(message,0); +} +void Commander::printError(){ + print(F("err"), 1); } \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h index c0bfd01e..b80cecbc 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -7,45 +7,133 @@ #include "../common/lowpass_filter.h" #include "commands.h" - -typedef void (* CommandCallback)(char*); +// callback function pointer definiton +typedef void (* CommandCallback)(char*); //!< command callback function pointer class Commander { public: /** - * Default constructor - setting all variabels to default values + * Default constructor receiving a serial interface that it uses to output the values to + * Also if the function run() is used it uses this serial instance to read the serial for user commands + * + * @param serial - Serial com port instance */ Commander(HardwareSerial &serial); + Commander(); + /** + * Function reading the serial port and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + */ void run(); + void run(HardwareSerial &reader); + /** + * 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 + * + * @param user_input - string of user inputs + */ void run(char* user_input); + /** + * Function adding a callback to the coomander withe the command id + * @param id - char command letter + * @param onCommand - function pointer void function(char*) + */ void add(char id , CommandCallback onCommand); - bool verbose = 1; - uint8_t decimal_places = 3; + // printing variables + bool verbose = 1; //!< flag signaling that the commands should output user understanable text + uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers // monitoring functions - HardwareSerial* com_port; //!< Serial terminal variable if provided + HardwareSerial* com_port = nullptr; //!< Serial terminal variable if provided + /** + * Function setting the configuration parameters of the motor, target value of the control loop + * and outputing them to the monitoring port( if available ) : + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: + * - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: + * - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PI controller P gain + * - I: velocity PI controller I gain + * - L: velocity PI controller voltage limit + * - R: velocity PI controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: anglex + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current angle + * - 3: current target value + * + * - Look into the documentation (docs.simplefoc.com) for more information. + * + * @param command String containing the user command + * + * returns 0 for error or 1 for executed command + */ void motor(FOCMotor* motor, char* user_cmd); void lpf(LowPassFilter* lpf, char* user_cmd); void pid(PIDController* pid, char* user_cmd); void variable(float* value, char* user_cmd); private: - CommandCallback call_list[20]; - char call_ids[20]; //!< Dictionary of nodes - int call_count = 0; - char received_chars[20] = {0}; - int rec_cnt = 0; - char cmd_scan_msg[2] = {CMD_SCAN,0}; + // Subscribed command callback variables + CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number + char call_ids[20]; //!< added callback commands + 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 + int rec_cnt = 0; //!< number of characters receives + char cmd_scan_msg[2] = {CMD_SCAN,0}; //!< scan message to be sent to the nodes - should be solved differently maybe - void verbosePrint(const char* message); - void verbosePrint(const __FlashStringHelper *message); - void printNumber(const float number, const bool newline = 0); + // serial printing functions + /** + * print the string message only if verbose mode on + * @param message - message to be printed + */ + void verbosePrint(const char* message); + /** + * Print the string message only if verbose mode on + * - Function handling the case for strings defined by F macro + * @param message - message to be printed + */ + void verbosePrint(const __FlashStringHelper *message); + /** + * print the numbers to the serial with desired decimal point number + * @param message - number to be printed + * @param newline - if needs lewline (1) otherwise (0) + */ + void print(const float number, const bool newline = 0); + + void printError(); + void print(const int number, const bool newline); + void print(const char* message, const bool newline); + void print(const __FlashStringHelper *message, const bool newline); + void print(const char message, const bool newline); }; From bd9973c62a21ab99ae886ec9b5e4f30a0b756fcf Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Feb 2021 18:42:38 +0100 Subject: [PATCH 104/749] fead refactored examples --- .../bluepill_position_control.ino | 41 ++---- .../bluepill_position_control.ino | 42 ++---- .../esp32_position_control.ino | 36 ++--- .../esp32_position_control.ino | 41 ++---- .../position_control/position_control.ino | 39 ++--- .../voltage_control/voltage_control.ino | 44 ++---- .../full_control_example.ino | 118 +++++++++++++++ .../full_control_example.ino | 87 +++++++++++ .../full_control_example.ino | 133 +++++++++++++++++ .../full_control_example.ino | 94 ++++++++++++ .../open_loop_position_example.ino | 46 ++---- .../open_loop_velocity_example.ino | 45 ++---- .../encoder/angle_control/angle_control.ino | 38 ++--- .../angle_control/angle_control.ino | 38 ++--- .../angle_control/angle_control.ino | 40 ++---- .../current_control/current_control.ino | 46 ++---- .../voltage_control/voltage_control.ino | 43 ++---- .../voltage_control/voltage_control.ino | 45 ++---- .../voltage_control/voltage_control.ino | 45 ++---- .../velocity_control/velocity_control.ino | 43 ++---- .../hall_sensor/velocity_control.ino | 41 ++---- .../velocity_control/velocity_control.ino | 43 ++---- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 6 +- .../alignment_and_cogging_test.ino | 0 .../find_pole_pairs_number.ino | 0 .../find_pole_pairs_number.ino | 0 .../find_sensor_offset_and_direction.ino | 0 .../commander_extend_example.ino | 55 +++++++ .../commander_no_serial.ino | 53 +++++++ .../commander_tune_custom_loop.ino | 79 ++++++++++ src/common/defaults.h | 6 +- src/common/foc_utils.cpp | 5 +- src/communication/Commander.cpp | 136 ++++++++++-------- src/communication/Commander.h | 54 +------ src/communication/commands.h | 2 + 35 files changed, 911 insertions(+), 673 deletions(-) create mode 100644 examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino create mode 100644 examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino create mode 100644 examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino create mode 100644 examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino rename examples/utils/{ => calibration}/alignment_and_cogging_test/alignment_and_cogging_test.ino (100%) rename examples/utils/{ => calibration}/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino (100%) rename examples/utils/{ => calibration}/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino (100%) rename examples/utils/{ => calibration}/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino (100%) create mode 100644 examples/utils/communication_test/commander_extend_example/commander_extend_example.ino create mode 100644 examples/utils/communication_test/commander_no_serial/commander_no_serial.ino create mode 100644 examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index b1e6fcef..64d23fd3 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -23,6 +23,14 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void doI(){encoder.handleIndex();} + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + + void setup() { // initialize encoder sensor hardware @@ -77,15 +85,14 @@ void setup() { // align encoder and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F(("Set the target angle using serial terminal:")); _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better @@ -104,31 +111,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } + command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index ee6a3a8d..9b42d3ec 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 @@ -26,6 +26,14 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5); // BLDCDriver6PWM(IN1_H, IN1_L, IN2_H, IN2_L, IN3_H, IN3_L, enable(optional)) //BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, PB12); + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + + void setup() { // initialise magnetic sensor hardware @@ -75,6 +83,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); @@ -104,33 +114,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } -} - - + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 7978567f..d7ef4c49 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -16,6 +16,12 @@ Encoder encoder = Encoder(4, 2, 1024); void doA(){encoder.handleA();} void doB(){encoder.handleB();} +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + void setup() { // initialize encoder sensor hardware @@ -71,6 +77,8 @@ void setup() { // align encoder and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); @@ -98,31 +106,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } + command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 39f322b6..34b677ee 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -24,6 +24,13 @@ MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); BLDCMotor motor = BLDCMotor(11); BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7); + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + void setup() { // initialise magnetic sensor hardware @@ -73,6 +80,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); @@ -102,33 +111,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } -} - - + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 3038b3ab..68344fe9 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -33,6 +33,13 @@ void doB(){encoder.handleB();} PciListenerImp listenerA(encoder.pinA, doA); PciListenerImp listenerB(encoder.pinB, doB); + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + void setup() { // initialise encoder hardware @@ -89,14 +96,14 @@ void setup() { // align encoder and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } -// angle set point variable -float target_angle = 0; void loop() { // main FOC algorithm function @@ -116,31 +123,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } + command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index fbb27a55..ebb338bc 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -39,6 +39,13 @@ void doB(){encoder.handleB();} PciListenerImp listenerA(encoder.pinA, doA); PciListenerImp listenerB(encoder.pinB, doB); + +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } + void setup() { // initialize encoder sensor hardware @@ -75,15 +82,15 @@ void setup() { motor.init(); // align sensor and start FOC motor.initFOC(); + + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } -// target voltage to be set to the motor -float target_voltage = 2; - void loop() { // main FOC algorithm function @@ -98,33 +105,6 @@ void loop() { // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - // communicate with the user - serialReceiveUserCommand(); -} - - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } + // user communication + command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino new file mode 100644 index 00000000..8931d82a --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino @@ -0,0 +1,118 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor1 = BLDCMotor(11); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8); + +// BLDC motor & driver instance +BLDCMotor motor2 = BLDCMotor(11); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7); + +// encoder instance +Encoder encoder1 = Encoder(2, A3, 500); +// channel A and B callbacks +void doA1(){encoder1.handleA();} +void doB1(){encoder1.handleB();} + +// encoder instance +Encoder encoder2 = Encoder(A1, A2, 500); +// channel A and B callbacks +void doA2(){encoder2.handleA();} +void doB2(){encoder2.handleB();} + +// commander communication instance +Commander command = Commander(Serial); +void doMotor1(char* cmd){ command.motor(&motor1, cmd); } +void doMotor2(char* cmd){ command.motor(&motor2, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder1.init(); + encoder1.enableInterrupts(doA1, doB1); + // initialize encoder sensor hardware + encoder2.init(); + encoder2.enableInterrupts(doA2, doB2); + // link the motor to the sensor + motor1.linkSensor(&encoder1); + motor2.linkSensor(&encoder2); + + + // driver config + // power supply voltage [V] + driver1.voltage_power_supply = 12; + driver1.init(); + // link driver + motor1.linkDriver(&driver1); + // power supply voltage [V] + driver2.voltage_power_supply = 12; + driver2.init(); + // link driver + motor2.linkDriver(&driver2); + + // set control loop type to be used + motor1.controller = MotionControlType::torque; + motor2.controller = MotionControlType::torque; + + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05; + motor1.PID_velocity.I = 1; + motor1.PID_velocity.D = 0; + // default voltage_power_supply + motor1.voltage_limit = 12; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05; + motor2.PID_velocity.I = 1; + motor2.PID_velocity.D = 0; + // default voltage_power_supply + motor2.voltage_limit = 12; + + // angle loop velocity limit + motor1.velocity_limit = 20; + motor2.velocity_limit = 20; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor1.useMonitoring(Serial); + motor2.useMonitoring(Serial); + + // initialise motor + motor1.init(); + // align encoder and start FOC + motor1.initFOC(); + + // initialise motor + motor2.init(); + // align encoder and start FOC + motor2.initFOC(); + + // set the inital target value + motor1.target = 2; + motor2.target = 2; + + // subscribe motor to the commander + command.add('A', doMotor1); + command.add('B', doMotor2); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Double motor sketch ready.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor1.loopFOC(); + motor2.loopFOC(); + + // iterative function setting the outter loop target + motor1.move(); + motor2.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino new file mode 100644 index 00000000..46a894c7 --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino @@ -0,0 +1,87 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// commander communication instance +Commander command = Commander(Serial); +void doMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set control loop type to be used + motor.controller = MotionControlType::torque; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05; + motor.PID_velocity.I = 1; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 20; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + // subscribe motor to the commander + command.add('M', doMotor); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // motor monitoring + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino new file mode 100644 index 00000000..26055855 --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino @@ -0,0 +1,133 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor1 = BLDCMotor(11); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8); + +// BLDC motor & driver instance +BLDCMotor motor2 = BLDCMotor(11); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7); + +// encoder instance +Encoder encoder1 = Encoder(12, 2, 500); +// channel A and B callbacks +void doA1(){encoder1.handleA();} +void doB1(){encoder1.handleB();} + +// encoder instance +Encoder encoder2 = Encoder(A5, A4, 500); +// channel A and B callbacks +void doA2(){encoder2.handleA();} +void doB2(){encoder2.handleB();} + + +// inline current sensor instance +InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2); + +// inline current sensor instance +InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); + +// commander communication instance +Commander command = Commander(Serial); +void doMotor1(char* cmd){ command.motor(&motor1, cmd); } +void doMotor2(char* cmd){ command.motor(&motor2, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder1.init(); + encoder1.enableInterrupts(doA1, doB1); + // initialize encoder sensor hardware + encoder2.init(); + encoder2.enableInterrupts(doA2, doB2); + // link the motor to the sensor + motor1.linkSensor(&encoder1); + motor2.linkSensor(&encoder2); + + + // driver config + // power supply voltage [V] + driver1.voltage_power_supply = 12; + driver1.init(); + // link driver + motor1.linkDriver(&driver1); + // power supply voltage [V] + driver2.voltage_power_supply = 12; + driver2.init(); + // link driver + motor2.linkDriver(&driver2); + + // set control loop type to be used + motor1.controller = MotionControlType::torque; + motor2.controller = MotionControlType::torque; + + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05; + motor1.PID_velocity.I = 1; + motor1.PID_velocity.D = 0; + // default voltage_power_supply + motor1.voltage_limit = 12; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05; + motor2.PID_velocity.I = 1; + motor2.PID_velocity.D = 0; + // default voltage_power_supply + motor2.voltage_limit = 12; + + // angle loop velocity limit + motor1.velocity_limit = 20; + motor2.velocity_limit = 20; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor1.useMonitoring(Serial); + motor2.useMonitoring(Serial); + + + // current sense init and linking + current_sense1.init(); + motor1.linkCurrentSense(¤t_sense1); + // current sense init and linking + current_sense2.init(); + motor2.linkCurrentSense(¤t_sense2); + + // initialise motor + motor1.init(); + // align encoder and start FOC + motor1.initFOC(); + + // initialise motor + motor2.init(); + // align encoder and start FOC + motor2.initFOC(); + + // set the inital target value + motor1.target = 2; + motor2.target = 2; + + // subscribe motor to the commander + command.add('A', doMotor1); + command.add('B', doMotor2); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Double motor sketch ready.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor1.loopFOC(); + motor2.loopFOC(); + + // iterative function setting the outter loop target + motor1.move(); + motor2.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino new file mode 100644 index 00000000..87064f63 --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino @@ -0,0 +1,94 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// inline current sensor instance +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); + +// commander communication instance +Commander command = Commander(Serial); +void doMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set control loop type to be used + motor.controller = MotionControlType::torque; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05; + motor.PID_velocity.I = 1; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 20; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // current sense init and linking + current_sense.init(); + motor.linkCurrentSense(¤t_sense); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + // subscribe motor to the commander + command.add('M', doMotor); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // motor monitoring + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index e1c4e31c..f0a0d84b 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -12,6 +12,13 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); +//target variable +float target_position = 0; + +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_position, cmd); } + void setup() { // driver config @@ -30,45 +37,20 @@ void setup() { // init motor hardware motor.init(); + // add target command T + command.add('T', doTarget); Serial.begin(115200); Serial.println("Motor ready!"); + Serial.println("Set target position [rad]"); _delay(1000); } -float target_position = 0; // [rad/s] - void loop() { // open loop angle movements // using motor.voltage_limit and motor.velocity_limit motor.move(target_position); - - // receive the used commands from serial - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_position = received_chars.toFloat(); - Serial.print("Target position: "); - Serial.println(target_position); - - // reset the command buffer - received_chars = ""; - } - } -} + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 885884f0..d0a3ea1e 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -12,6 +12,14 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +//target variable +float target_velocity = 0; + +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } + void setup() { // driver config @@ -31,44 +39,21 @@ void setup() { // init motor hardware motor.init(); + // add target command T + command.add('T', doTarget); + Serial.begin(115200); Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); _delay(1000); } -float target_velocity = 0; // [rad/s] - void loop() { + // open loop velocity movement // using motor.voltage_limit and motor.velocity_limit motor.move(target_velocity); - // receive the used commands from serial - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_velocity = received_chars.toFloat(); - Serial.print("Target velocity "); - Serial.println(target_velocity); - - // reset the command buffer - received_chars = ""; - } - } + // user communication + command.run(); } diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 5a352b6e..9fdba265 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -46,6 +46,11 @@ void doIndex(){encoder.handleIndex();} // If no available hadware interrupt pins use the software interrupt PciListenerImp listenerIndex(encoder.index_pin, doIndex); +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } void setup() { @@ -104,15 +109,14 @@ void setup() { // align encoder and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better @@ -131,31 +135,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } + command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 9c6c4192..3e1a4085 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -39,6 +39,11 @@ void doC(){sensor.handleC();} // If no available hadware interrupt pins use the software interrupt PciListenerImp listenC(sensor.pinC, doC); +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } void setup() { @@ -98,15 +103,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better @@ -125,31 +129,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } + command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 5d346243..75b6f84c 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -23,6 +23,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_angle, cmd); } + void setup() { // initialise magnetic sensor hardware @@ -73,6 +79,8 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); @@ -102,33 +110,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_angle = received_chars.toFloat(); - Serial.print("Target angle: "); - Serial.println(target_angle); - - // reset the command buffer - received_chars = ""; - } - } -} - - + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index 71e86b4b..f5cebc90 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -13,16 +13,19 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // encoder instance Encoder encoder = Encoder(2, 3, 500); - -// Interrupt routine intialisation // channel A and B callbacks void doA(){encoder.handleA();} void doB(){encoder.handleB();} - // current sensor InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); +// voltage set point variable +float target_current = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_current, cmd); } + void setup() { // initialize encoder sensor hardware @@ -71,14 +74,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } -// target voltage to be set to the motor -float target_current = 0; - void loop() { // main FOC algorithm function @@ -92,34 +95,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_current); - - // communicate with the user - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_current = received_chars.toFloat(); - Serial.print("Target current: "); - Serial.println(target_current); - - // reset the command buffer - received_chars = ""; - } - } + // user communication + command.run(); } \ No newline at end of file diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 40f90142..cff287c7 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -25,6 +25,12 @@ Encoder encoder = Encoder(2, 3, 8192); void doA(){encoder.handleA();} void doB(){encoder.handleB();} +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } + void setup() { // initialize encoder sensor hardware @@ -59,14 +65,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } -// target voltage to be set to the motor -float target_voltage = 2; - void loop() { // main FOC algorithm function @@ -80,34 +86,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - - // communicate with the user - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } + // user communication + command.run(); } \ No newline at end of file diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index cf64070f..36330b22 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -26,6 +26,13 @@ void doA(){sensor.handleA();} void doB(){sensor.handleB();} void doC(){sensor.handleC();} + +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } + void setup() { // initialize encoder sensor hardware @@ -60,13 +67,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } -// target voltage to be set to the motor -float target_voltage = 2; void loop() { @@ -81,34 +89,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - - // communicate with the user - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } -} + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index 13273b92..64e0b629 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -22,6 +22,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } + void setup() { // initialise magnetic sensor hardware @@ -51,14 +57,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); _delay(1000); } -// target voltage to be set to the motor -float target_voltage = 2; - void loop() { // main FOC algorithm function @@ -73,33 +79,6 @@ void loop() { // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - // communicate with the user - serialReceiveUserCommand(); -} - - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_voltage = received_chars.toFloat(); - Serial.print("Target voltage: "); - Serial.println(target_voltage); - - // reset the command buffer - received_chars = ""; - } - } -} + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index 27d97576..0125217d 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -48,6 +48,13 @@ void doIndex(){encoder.handleIndex();} PciListenerImp listenerIndex(encoder.index_pin, doIndex); +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } + + void setup() { // initialize encoder sensor hardware @@ -99,13 +106,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } -// velocity set point variable -float target_velocity = 0; void loop() { // main FOC algorithm function @@ -125,32 +133,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_velocity = received_chars.toFloat(); - Serial.print("Target velocity: "); - Serial.println(target_velocity); - - // reset the command buffer - received_chars = ""; - } - } -} - + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 0f3441e7..239dc233 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -39,6 +39,11 @@ void doC(){sensor.handleC();} // If no available hadware interrupt pins use the software interrupt PciListenerImp listenerIndex(sensor.pinC, doC); +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } void setup() { @@ -89,13 +94,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } -// velocity set point variable -float target_velocity = 0; void loop() { // main FOC algorithm function @@ -115,32 +121,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_velocity = received_chars.toFloat(); - Serial.print("Target velocity: "); - Serial.println(target_velocity); - - // reset the command buffer - received_chars = ""; - } - } -} - + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index f47bd078..099a190c 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -25,6 +25,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(50); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } + void setup() { // initialise magnetic sensor hardware @@ -70,14 +76,14 @@ void setup() { // align sensor and start FOC motor.initFOC(); + // add target command T + command.add('T', doTarget); + Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); _delay(1000); } -// velocity set point variable -float target_velocity = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better @@ -96,32 +102,5 @@ void loop() { // motor.monitor(); // user communication - serialReceiveUserCommand(); -} - -// utility function enabling serial communication with the user to set the target values -// this function can be implemented in serialEvent function as well -void serialReceiveUserCommand() { - - // a string to hold incoming data - static String received_chars; - - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the string buffer: - received_chars += inChar; - // end of user input - if (inChar == '\n') { - - // change the motor target - target_velocity = received_chars.toFloat(); - Serial.print("Target velocity: "); - Serial.println(target_velocity); - - // reset the command buffer - received_chars = ""; - } - } -} - + command.run(); +} \ No newline at end of file diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index 0ebfc51a..b0a24d25 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -49,12 +49,12 @@ #include -const char ssid[] = "myssid"; // your network SSID (name) +const char ssid[] = "myssid"; // your network SSID (name) const char pass[] = "mypassword"; // your network password WiFiUDP Udp; IPAddress outIp(192,168,1,17); // remote IP (not needed for receive) -const unsigned int outPort = 8000; // remote port (not needed for receive) -const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) +const unsigned int outPort = 8000; // remote port (not needed for receive) +const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) OSCErrorCode error; diff --git a/examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino similarity index 100% rename from examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino rename to examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino diff --git a/examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino similarity index 100% rename from examples/utils/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino rename to examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino diff --git a/examples/utils/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 similarity index 100% rename from examples/utils/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino rename to examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino diff --git a/examples/utils/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 similarity index 100% rename from examples/utils/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino rename to examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino diff --git a/examples/utils/communication_test/commander_extend_example/commander_extend_example.ino b/examples/utils/communication_test/commander_extend_example/commander_extend_example.ino new file mode 100644 index 00000000..3168feda --- /dev/null +++ b/examples/utils/communication_test/commander_extend_example/commander_extend_example.ino @@ -0,0 +1,55 @@ +/** + * Simple example of custom commands that have nothing to do with the simple foc library + */ + +#include + +// instantiate the commander +Commander command = Commander(Serial); + +// led control function +void doLed(char* cmd){ + if(cmd[0] == CMD_SCAN ) Serial.println("led control"); + else if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); + else digitalWrite(LED_BUILTIN, LOW); +}; +// get analog input +void doAnalog(char* cmd){ + if(cmd[0] == CMD_SCAN ) Serial.println("analog read A0-A4"); + else if (cmd[0] == '0') Serial.println(analogRead(A0)); + else if (cmd[0] == '1') Serial.println(analogRead(A1)); + else if (cmd[0] == '2') Serial.println(analogRead(A2)); + else if (cmd[0] == '3') Serial.println(analogRead(A3)); + else if (cmd[0] == '4') Serial.println(analogRead(A4)); +}; + +void setup() { + // define pins + pinMode(LED_BUILTIN, OUTPUT); + pinMode(A0, INPUT); + pinMode(A1, INPUT); + pinMode(A2, INPUT); + pinMode(A3, INPUT); + pinMode(A4, INPUT); + + // Serial port to be used + Serial.begin(115200); + + // add new commands + command.add('L', doLed); + command.add('A', doAnalog); + + Serial.println(F("Commander listening")); + Serial.println(F(" - Send ? to see the node list...")); + Serial.println(F(" - Send L0 to turn the led off and L1 to turn it off")); + Serial.println(F(" - Send A0-A4 to read the analog pins")); + _delay(1000); +} + + +void loop() { + + // user communication + command.run(); + _delay(300); +} \ No newline at end of file diff --git a/examples/utils/communication_test/commander_no_serial/commander_no_serial.ino b/examples/utils/communication_test/commander_no_serial/commander_no_serial.ino new file mode 100644 index 00000000..301b3bbb --- /dev/null +++ b/examples/utils/communication_test/commander_no_serial/commander_no_serial.ino @@ -0,0 +1,53 @@ +/** + * Simple example of how to use the commander without serial - using just strings + */ + +#include + +// instantiate the commander +Commander command = Commander(); + +// led control function +void doLed(char* cmd){ + if(cmd[0] == CMD_SCAN ) Serial.println("led control"); + else if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); + else digitalWrite(LED_BUILTIN, LOW); +}; +// get analog input +void doAnalog(char* cmd){ + if(cmd[0] == CMD_SCAN ) Serial.println("analog read A0-A1"); + else if (cmd[0] == '0') Serial.println(analogRead(A0)); + else if (cmd[0] == '1') Serial.println(analogRead(A1)); +}; + +void setup() { + // define pins + pinMode(LED_BUILTIN, OUTPUT); + pinMode(A0, INPUT); + pinMode(A1, INPUT); + + // Serial port to be used + Serial.begin(115200); + + // add new commands + command.add('L', doLed); + command.add('A', doAnalog); + + Serial.println(F("Commander running")); + _delay(1000); +} + + +void loop() { + // user communication + command.run("?"); + _delay(2000); + command.run("L0"); + _delay(1000); + command.run("A0"); + _delay(1000); + command.run("A1"); + _delay(1000); + command.run("L1"); + _delay(1000); +} \ No newline at end of file diff --git a/examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino new file mode 100644 index 00000000..40643366 --- /dev/null +++ b/examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -0,0 +1,79 @@ +/** + * A simple example to show how to use the commander with the control loops outside of the scope of the SimpleFOC library +*/ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA() { encoder.handleA(); } +void doB() { encoder.handleB(); } + +// target voltage to be set to the motor +float target_velocity = 0; + +// PID controllers and low pass filters +PIDController PIDv{0.05, 1, 0, 100000000, 12}; +LowPassFilter LPFv{0.01}; + +//add communication +Commander command = Commander(Serial); +void doController(char* cmd) { command.pid(&PIDv, cmd); } +void doFilter(char* cmd) { command.lpf(&LPFv, cmd); } +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } + + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set motion control loop to be used ( doing nothing ) + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + motor.useMonitoring(Serial); + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // subscribe the new commands + command.add('C', doController); + command.add('F', doFilter); + command.add('T', doTarget); + + _delay(1000); + Serial.println(F("Commander listening")); + Serial.println(F(" - Send ? to see the node list...")); +} + + + +void loop() { + // looping foc + motor.loopFOC(); + + // calculate voltage + float target_voltage = PIDv(target_velocity - LPFv(motor.shaft_velocity)); + // set the voltage + motor.move(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/src/common/defaults.h b/src/common/defaults.h index fa1ea6ad..27c2cbcc 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -20,12 +20,12 @@ #define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant #else // for stm32, due, teensy, esp32 and similar -#define DEF_PID_CURR_P 5 //!< default PID controller P value -#define DEF_PID_CURR_I 1000.0 //!< default PID controller I value +#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_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.002 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant #endif // default current limit values #define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 3d36b1cf..7653f9be 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -59,10 +59,11 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat long i; - float x, y; + float y; + // float x; // const float f = 1.5F; // better precision - x = number * 0.5F; + // x = number * 0.5F; y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 9c66556f..ac67e74f 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -54,25 +54,39 @@ void Commander::run(HardwareSerial &serial){ void Commander::run(char* user_input){ // execute the user command char id = user_input[0]; - if(id == CMD_SCAN) - for(int i=0; i < call_count; i++){ - print(call_ids[i], 0); - print(":", 0); - call_list[i](cmd_scan_msg); - } - else - for(int i=0; i < call_count; i++){ - if(id == call_ids[i]){ - call_list[i](&user_input[1]); - break; + switch(id){ + case CMD_SCAN: + for(int i=0; i < call_count; i++){ + dump(call_ids[i], 0); + dump(":", 0); + call_list[i](cmd_scan_msg); } + break; + case CMD_VERBOSE: + if(user_input[1] != '\n') verbose = atoi(&user_input[1]); + if(verbose) dump(F("Verb. on!"),1); + else dump(F("Verb. off"),1); + break; + case CMD_DECIMAL: + if(user_input[1] != '\n') decimal_places = atoi(&user_input[1]); + verbosePrint(F("Decimal:")); + dump(decimal_places,1); + break; + default: + for(int i=0; i < call_count; i++){ + if(id == call_ids[i]){ + call_list[i](&user_input[1]); + break; + } + } + break; } } void Commander::motor(FOCMotor* motor, char* user_command) { // if empty string if( user_command[0] == CMD_SCAN ){ - print(F("mot"),1); + dump(F("mot"),1); return; } @@ -119,7 +133,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // change velocity pid limit if in voltage mode and no phase resistance set if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value; } - print(motor->voltage_limit,1); + dump(motor->voltage_limit,1); break; case SCMD_LIM_CURR: // current limit verbosePrint(F("curr: ")); @@ -130,7 +144,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // 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; } - print(motor->current_limit,1); + dump(motor->current_limit,1); break; case SCMD_LIM_VEL: // velocity limit verbosePrint(F("vel: ")); @@ -138,7 +152,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->velocity_limit = value; motor->P_angle.limit = value; } - print(motor->velocity_limit,1); + dump(motor->velocity_limit,1); break; default: printError(); @@ -151,7 +165,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_DOWNSAMPLE: verbosePrint(F("downsample: ")); if(!GET) motor->motion_downsample = value; - print((int)motor->motion_downsample, 1); + dump((int)motor->motion_downsample, 1); break; default: // change control type @@ -159,19 +173,19 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->controller = (MotionControlType)value; switch(motor->controller){ case MotionControlType::torque: - print(F("torque"),1); + dump(F("torque"),1); break; case MotionControlType::velocity: - print(F("vel"),1); + dump(F("vel"),1); break; case MotionControlType::angle: - print(F("angle"),1); + dump(F("angle"),1); break; case MotionControlType::velocity_openloop: - print(F("vel open"),1); + dump(F("vel open"),1); break; case MotionControlType::angle_openloop: - print(F("angle open"),1); + dump(F("angle open"),1); break; } break; @@ -184,13 +198,13 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ case TorqueControlType::voltage: - print(F("volt"),1); + dump(F("volt"),1); break; case TorqueControlType::current: - print(F("curr"),1); + dump(F("curr"),1); break; case TorqueControlType::foc_current: - print(F("foc"),1); + dump(F("foc"),1); break; } break; @@ -198,7 +212,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // enable/disable verbosePrint(F("Status: ")); if(!GET) (bool)value ? motor->enable() : motor->disable(); - print(motor->enabled,1); + dump(motor->enabled,1); break; case CMD_RESIST: // enable/disable @@ -210,8 +224,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->PID_velocity.limit= motor->current_limit; } } - if(_isset(motor->phase_resistance)) print(motor->phase_resistance,1); - else print(0,0); + if(_isset(motor->phase_resistance)) dump(motor->phase_resistance,1); + else dump(0,0); break; case CMD_SENSOR: // Sensor zero offset @@ -220,12 +234,12 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_SENS_MECH_OFFSET: // zero offset verbosePrint(F("offset: ")); if(!GET) motor->sensor_offset = value; - print(motor->sensor_offset,1); + dump(motor->sensor_offset,1); break; case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch verbosePrint(F("el. offset: ")); if(!GET) motor->zero_electric_angle = value; - print(motor->zero_electric_angle,1); + dump(motor->zero_electric_angle,1); break; default: printError(); @@ -239,31 +253,31 @@ void Commander::motor(FOCMotor* motor, char* user_command) { switch((uint8_t)value){ case 0: // get target verbosePrint(F("target: ")); - print(motor->target,1); + dump(motor->target,1); break; case 1: // get voltage q verbosePrint(F("Vq: ")); - print(motor->voltage.q,1); + dump(motor->voltage.q,1); break; case 2: // get voltage d verbosePrint(F("Vd: ")); - print(motor->voltage.q,1); + dump(motor->voltage.q,1); break; case 3: // get current q verbosePrint(F("Cq: ")); - print(motor->voltage.q,1); + dump(motor->voltage.q,1); break; case 4: // get current d verbosePrint(F("Cd: ")); - print(motor->voltage.q,1); + dump(motor->voltage.q,1); break; case 5: // get velocity verbosePrint(F("vel: ")); - print(motor->shaft_velocity,1); + dump(motor->shaft_velocity,1); break; case 6: // get angle verbosePrint(F("Angle: ")); - print(motor->shaft_angle,1); + dump(motor->shaft_angle,1); break; default: printError(); @@ -273,18 +287,18 @@ void Commander::motor(FOCMotor* motor, char* user_command) { case SCMD_DOWNSAMPLE: verbosePrint(F("downsample: ")); if(!GET) motor->monitor_downsample = value; - print((int)motor->monitor_downsample,1); + dump((int)motor->monitor_downsample,1); break; case SCMD_CLEAR: for(int i=0; i<7; i++) motor->monitor_variables[i] = 0; - print(F("clear"),1); + dump(F("clear"),1); break; case SCMD_SET: for(int i=0; i<7; i++){ motor->monitor_variables[i] = user_command[value_index+i] - '0'; - print(motor->monitor_variables[i],0); + dump(motor->monitor_variables[i],0); } - print("",1); + dump("",1); break; default: printError(); @@ -294,13 +308,13 @@ void Commander::motor(FOCMotor* motor, char* user_command) { default: // target change verbosePrint(F("Target: ")); motor->target = atof(user_command); - print(motor->target,1); + dump(motor->target,1); } } void Commander::pid(PIDController* pid, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - print(F("pid"),1); + dump(F("pid"),1); return; } char cmd = user_cmd[0]; @@ -311,27 +325,27 @@ void Commander::pid(PIDController* pid, char* user_cmd){ case SCMD_PID_P: // P gain change verbosePrint("P: "); if(!GET) pid->P = value; - print(pid->P,1); + dump(pid->P,1); break; case SCMD_PID_I: // I gain change verbosePrint("I: "); if(!GET) pid->I = value; - print(pid->I,1); + dump(pid->I,1); break; case SCMD_PID_D: // D gain change verbosePrint("D: "); if(!GET) pid->D = value; - print(pid->D,1); + dump(pid->D,1); break; case SCMD_PID_RAMP: // ramp change verbosePrint("ramp: "); if(!GET) pid->output_ramp = value; - print(pid->output_ramp,1); + dump(pid->output_ramp,1); break; case SCMD_PID_LIM: // limit change verbosePrint("limit: "); if(!GET) pid->limit = value; - print(pid->limit,1); + dump(pid->limit,1); break; default: printError(); @@ -341,7 +355,7 @@ void Commander::pid(PIDController* pid, char* user_cmd){ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - print(F("lpf"),1); + dump(F("lpf"),1); return; } char cmd = user_cmd[0]; @@ -352,7 +366,7 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ case SCMD_LPF_TF: // Tf value change verbosePrint(F("Tf: ")); if(!GET) lpf->Tf = value; - print(lpf->Tf,1); + dump(lpf->Tf,1); break; default: printError(); @@ -362,36 +376,36 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ void Commander::variable(float* value, char* user_cmd){ if( user_cmd[0] == CMD_SCAN ){ - print(F("var"),1); + dump(F("var"),1); return; } bool GET = user_cmd[0] == '\n'; if(!GET) *value = atof(user_cmd); - print(*value,1); + dump(*value,1); } -void Commander::print(const int number, const bool newline){ +void Commander::dump(const int number, const bool newline){ if(!com_port) return; if(newline) com_port->println(number); else com_port->print(number); } -void Commander::print(const float number, const bool newline){ +void Commander::dump(const float number, const bool newline){ if(!com_port) return; - if(newline) com_port->println(number,decimal_places); - else com_port->print(number,decimal_places); + if(newline) com_port->println((float)number,(int)decimal_places); + else com_port->print((float)number,(int)decimal_places); } -void Commander::print(const char* message, const bool newline){ +void Commander::dump(const char* message, const bool newline){ if(!com_port) return; if(newline) com_port->println(message); else com_port->print(message); } -void Commander::print(const __FlashStringHelper *message, const bool newline){ +void Commander::dump(const __FlashStringHelper *message, const bool newline){ if(!com_port) return; if(newline) com_port->println(message); else com_port->print(message); } -void Commander::print(const char message, const bool newline){ +void Commander::dump(const char message, const bool newline){ if(!com_port) return; if(newline) com_port->println(message); else com_port->print(message); @@ -399,11 +413,11 @@ void Commander::print(const char message, const bool newline){ void Commander::verbosePrint(const char* message){ - if(verbose) print(message,0); + if(verbose) dump(message,0); } void Commander::verbosePrint(const __FlashStringHelper *message){ - if(verbose) print(message,0); + if(verbose) dump(message,0); } void Commander::printError(){ - print(F("err"), 1); + dump(F("err"), 1); } \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h index b80cecbc..4e376793 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -28,7 +28,7 @@ class Commander */ void run(); void run(HardwareSerial &reader); - /** + /**x * 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 * @@ -51,47 +51,6 @@ class Commander HardwareSerial* com_port = nullptr; //!< Serial terminal variable if provided /** - * Function setting the configuration parameters of the motor, target value of the control loop - * and outputing them to the monitoring port( if available ) : - * - configure PID controller constants - * - change motion control loops - * - monitor motor variabels - * - set target values - * - check all the configuration values - * - * To check the config value just enter the command letter. - * For example: - * - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - * - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PI controller P gain - * - I: velocity PI controller I gain - * - L: velocity PI controller voltage limit - * - R: velocity PI controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: anglex - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * - * - Look into the documentation (docs.simplefoc.com) for more information. - * - * @param command String containing the user command - * - * returns 0 for error or 1 for executed command */ void motor(FOCMotor* motor, char* user_cmd); void lpf(LowPassFilter* lpf, char* user_cmd); @@ -126,14 +85,13 @@ class Commander * @param message - number to be printed * @param newline - if needs lewline (1) otherwise (0) */ - void print(const float number, const bool newline = 0); + void dump(const float number, const bool newline = 1); + void dump(const int number, const bool newline = 1); + void dump(const char* message, const bool newline = 1); + void dump(const __FlashStringHelper *message, const bool newline = 1); + void dump(const char message, const bool newline = 1); void printError(); - - void print(const int number, const bool newline); - void print(const char* message, const bool newline); - void print(const __FlashStringHelper *message, const bool newline); - void print(const char message, const bool newline); }; diff --git a/src/communication/commands.h b/src/communication/commands.h index bd4288f9..5504c87f 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -17,6 +17,8 @@ #define CMD_RESIST 'R' //!< motor phase resistance #define CMD_SCAN '?' //!< command scaning the network - only for commander + #define CMD_VERBOSE '@' //!< command setting output mode - only for commander + #define CMD_DECIMAL '#' //!< command setting decimal places - only for commander // subcomands //pid - lpf From 8de22cf6cc006f7aae970045840267e7f34ab7d0 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Feb 2021 18:50:17 +0100 Subject: [PATCH 105/749] refactored names and typo position control --- .../double_full_control_example.ino} | 0 .../single_full_control_example.ino} | 0 .../double_full_control_example.ino} | 0 .../sngle_full_control_example.ino} | 0 .../magnetic_sensor/angle_control/angle_control.ino | 2 -- 5 files changed, 2 deletions(-) rename examples/hardware_specific_examples/SimpleFOCShield/version v1/{double/full_control_example/full_control_example.ino => double_full_control_example/double_full_control_example.ino} (100%) rename examples/hardware_specific_examples/SimpleFOCShield/version v1/{single/full_control_example/full_control_example.ino => single_full_control_example/single_full_control_example.ino} (100%) rename examples/hardware_specific_examples/SimpleFOCShield/version v2/{double/full_control_example/full_control_example.ino => double_full_control_example/double_full_control_example.ino} (100%) rename examples/hardware_specific_examples/SimpleFOCShield/version v2/{single/full_control_example/full_control_example.ino => single_full_control_example/sngle_full_control_example.ino} (100%) diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v1/double_full_control_example/double_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v1/double/full_control_example/full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version v1/double_full_control_example/double_full_control_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v1/single_full_control_example/single_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v1/single/full_control_example/full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version v1/single_full_control_example/single_full_control_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v2/double_full_control_example/double_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v2/double/full_control_example/full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version v2/double_full_control_example/double_full_control_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version v2/single_full_control_example/sngle_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v2/single/full_control_example/full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version v2/single_full_control_example/sngle_full_control_example.ino diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 75b6f84c..b3d74211 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -87,8 +87,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; void loop() { From a231a1e352fe5d42d691448b62f8a61982fdfdea Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Feb 2021 18:54:34 +0100 Subject: [PATCH 106/749] yet another typo --- .../double_full_control_example/double_full_control_example.ino | 0 .../single_full_control_example/single_full_control_example.ino | 0 .../double_full_control_example/double_full_control_example.ino | 0 .../single_full_control_example/single_full_control_example.ino} | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename examples/hardware_specific_examples/SimpleFOCShield/{version v1 => version_v1}/double_full_control_example/double_full_control_example.ino (100%) rename examples/hardware_specific_examples/SimpleFOCShield/{version v1 => version_v1}/single_full_control_example/single_full_control_example.ino (100%) rename examples/hardware_specific_examples/SimpleFOCShield/{version v2 => version_v2}/double_full_control_example/double_full_control_example.ino (100%) rename examples/hardware_specific_examples/SimpleFOCShield/{version v2/single_full_control_example/sngle_full_control_example.ino => version_v2/single_full_control_example/single_full_control_example.ino} (100%) diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v1/double_full_control_example/double_full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v1/single_full_control_example/single_full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino 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 similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v2/double_full_control_example/double_full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version v2/single_full_control_example/sngle_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino similarity index 100% rename from examples/hardware_specific_examples/SimpleFOCShield/version v2/single_full_control_example/sngle_full_control_example.ino rename to examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino From 43e26497ebc9efa555661cd327ba212401c6dc8d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 22 Feb 2021 21:44:06 +0100 Subject: [PATCH 107/749] Added step/dir interface --- .../commander_extend_example.ino | 0 .../commander_no_serial.ino | 0 .../commander_tune_custom_loop.ino | 0 .../step_dir_listener_simple.ino | 36 +++++++ .../step_dir_listener_software_interrupt.ino | 44 +++++++++ .../step_dir_motor_example.ino | 98 +++++++++++++++++++ keywords.txt | 6 ++ src/SimpleFOC.h | 1 + src/communication/StepDirListener.cpp | 40 ++++++++ src/communication/StepDirListener.h | 29 ++++++ src/sensors/Encoder.cpp | 6 +- 11 files changed, 257 insertions(+), 3 deletions(-) rename examples/utils/communication_test/{ => commander}/commander_extend_example/commander_extend_example.ino (100%) rename examples/utils/communication_test/{ => commander}/commander_no_serial/commander_no_serial.ino (100%) rename examples/utils/communication_test/{ => commander}/commander_tune_custom_loop/commander_tune_custom_loop.ino (100%) create mode 100644 examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino create mode 100644 examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino create mode 100644 examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino create mode 100644 src/communication/StepDirListener.cpp create mode 100644 src/communication/StepDirListener.h diff --git a/examples/utils/communication_test/commander_extend_example/commander_extend_example.ino b/examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino similarity index 100% rename from examples/utils/communication_test/commander_extend_example/commander_extend_example.ino rename to examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino diff --git a/examples/utils/communication_test/commander_no_serial/commander_no_serial.ino b/examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino similarity index 100% rename from examples/utils/communication_test/commander_no_serial/commander_no_serial.ino rename to examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino diff --git a/examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino similarity index 100% rename from examples/utils/communication_test/commander_tune_custom_loop/commander_tune_custom_loop.ino rename to examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino b/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino new file mode 100644 index 00000000..0c9bf440 --- /dev/null +++ b/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino @@ -0,0 +1,36 @@ +/** + * A simple example of reading step/dir communication + * - this example uses hadware interrupts +*/ + +#include + +// angle +float received_angle = 0; + +// StepDirListener( step_pin, dir_pin, counts_per_revolution) +StepDirListener step_dir = StepDirListener(2, 3, 200); +void onStep() { step_dir.handle(); } + +void setup() { + + Serial.begin(115200); + + // init step and dir pins + step_dir.init(); + // enable interrupts + step_dir.enableInterrupt(onStep); + // attach the variable to be updated on each step (optional) + // the same can be done asynchronously by caling step_dir.getValue(); + step_dir.attach(&received_angle); + + Serial.println(F("Step/Dir listenning.")); + _delay(1000); +} + +void loop() { + Serial.print(received_angle); + Serial.print("\t"); + Serial.println(step_dir.getValue()); + _delay(500); +} \ No newline at end of file diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino new file mode 100644 index 00000000..d1edaa0d --- /dev/null +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -0,0 +1,44 @@ +/** + * A simple example of reading step/dir communication + * - this example uses software interrupts - this code is intended primarily + * for Arduino UNO/Mega and similar boards with very limited number of interrupt pins +*/ + +#include +// software interrupt library +#include +#include + + +// angle +float received_angle = 0; + +// StepDirListener( step_pin, dir_pin, counts_per_revolution) +StepDirListener step_dir = StepDirListener(4, 5, 200); +void onStep() { step_dir.handle(); } + +// If no available hadware interrupt pins use the software interrupt +PciListenerImp listenStep(step_dir.pin_step, onStep); + +void setup() { + + Serial.begin(115200); + + // init step and dir pins + step_dir.init(); + // enable software interrupts + PciManager.registerListener(&listenStep); + // attach the variable to be updated on each step (optional) + // the same can be done asynchronously by caling step_dir.getValue(); + step_dir.attach(&received_angle); + + Serial.println(F("Step/Dir listenning.")); + _delay(1000); +} + +void loop() { + Serial.print(received_angle); + Serial.print("\t"); + Serial.println(step_dir.getValue()); + _delay(500); +} \ No newline at end of file diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino new file mode 100644 index 00000000..804c7753 --- /dev/null +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -0,0 +1,98 @@ +/** + * A position control example using step/dir interface to update the motor position + */ + +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(10, 5, 6, 8); + +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA() { encoder.handleA(); } +void doB() { encoder.handleB(); } + +// StepDirListener( step_pin, dir_pin, counts_per_revolution) +StepDirListener step_dir = StepDirListener(A4, A5, 200); +void onStep() { step_dir.handle(); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + // index search velocity [rad/s] + motor.velocity_index_search = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::angle; + + // contoller configuration + // default parameters in defaults.h + // velocity PI controller parameters + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle P controller + motor.P_angle.P = 10; + // maximal velocity of the position control + motor.velocity_limit = 100; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // init step and dir pins + step_dir.init(); + // enable interrupts + step_dir.enableInterrupt(onStep); + // attach the variable to be updated on each step (optional) + // the same can be done asynchronously by caling motor.move(step_dir.getValue()); + step_dir.attach(&motor.target); + + Serial.println(F("Motor ready.")); + Serial.println(F("Listening to step/dir commands!")); + _delay(1000); +} + +void loop() { + + // main FOC algorithm function + motor.loopFOC(); + + // Motion control function + motor.move(); +} \ No newline at end of file diff --git a/keywords.txt b/keywords.txt index 774a5c57..b32a3c89 100644 --- a/keywords.txt +++ b/keywords.txt @@ -19,6 +19,7 @@ LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 +StepDirListener KEYWORD1 initFOC KEYWORD2 @@ -83,6 +84,11 @@ driverAlign KEYWORD2 driverSync KEYWORD2 add KEYWORD2 run KEYWORD2 +attach KEYWORD2 +enableInterrupt KEYWORD2 +getValue KEYWORD2 +handle KEYWORD2 + current KEYWORD2 diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 5b1c4ee0..6b7afbb2 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,5 +109,6 @@ void loop() { #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" #include "communication/Commander.h" +#include "communication/StepDirListener.h" #endif diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp new file mode 100644 index 00000000..3bf9d3fe --- /dev/null +++ b/src/communication/StepDirListener.cpp @@ -0,0 +1,40 @@ +#include "StepDirListener.h" + +StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _step_per_rotation){ + + pin_step = _pinStep; + pin_dir = _pinDir; + + step_per_rotation = _step_per_rotation; + +} + +void StepDirListener::init(){ + pinMode(pin_dir, INPUT); + pinMode(pin_step, INPUT_PULLUP); + count = 0; +} + +void StepDirListener::enableInterrupt(void (*doA)()){ + attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE); +} + +void StepDirListener::attach(float* variable){ + attached_variable = variable; +} + +void StepDirListener::handle(){ + bool step = digitalRead(pin_step); + if(step && step != step_active){ + if(digitalRead(pin_dir)) + count++; + else + count--; + } + step_active = step; + if(attached_variable) *attached_variable = getValue(); +} + +float StepDirListener::getValue(){ + return (float) count / step_per_rotation * _2PI; +} \ No newline at end of file diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h new file mode 100644 index 00000000..0eff2bbc --- /dev/null +++ b/src/communication/StepDirListener.h @@ -0,0 +1,29 @@ +#ifndef STEPDIR_H +#define STEPDIR_H + +#include "Arduino.h" +#include "../common/foc_utils.h" + +class StepDirListener +{ + public: + StepDirListener(int pinStep, int pinDir, float step_per_rotation); + void enableInterrupt(void (*doA)()); + void init(); + void handle(); + + float getValue(); + + void attach(float* variable); + + int pin_step; + int pin_dir; + long count; + + private: + float* attached_variable = nullptr; + long step_per_rotation; + bool step_active = 0; +}; + +#endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 76e17429..5b196d57 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -40,7 +40,7 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ // Encoder interrupt callback functions // A channel void Encoder::handleA() { - int A = digitalRead(pinA); + bool A = digitalRead(pinA); switch (quadrature){ case Quadrature::ON: // CPR = 4xPPR @@ -61,7 +61,7 @@ void Encoder::handleA() { } // B channel void Encoder::handleB() { - int B = digitalRead(pinB); + bool B = digitalRead(pinB); switch (quadrature){ case Quadrature::ON: // // CPR = 4xPPR @@ -84,7 +84,7 @@ void Encoder::handleB() { // Index channel void Encoder::handleIndex() { if(hasIndex()){ - int I = digitalRead(index_pin); + bool I = digitalRead(index_pin); if(I && !I_active){ index_found = true; // align encoder on each index From d84e8752e7373298a6b07b79e40d328dd3194db1 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 22 Feb 2021 21:45:37 +0100 Subject: [PATCH 108/749] readme updated --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 4b3a2a6b..138cb4b0 100644 --- a/README.md +++ b/README.md @@ -42,6 +42,8 @@ Therefore this is an attempt to: > - 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` From 0ce7c0cac9bd903b8a200ef24b9c71f724dc50da Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 23 Feb 2021 16:08:56 +0100 Subject: [PATCH 109/749] Added step/dir interface and refactored commander --- README.md | 3 +- .../bluepill_position_control.ino | 2 +- .../bluepill_position_control.ino | 2 +- .../full_control_serial.ino | 4 +- .../full_control_serial.ino | 4 +- .../esp32_position_control.ino | 2 +- .../esp32_position_control.ino | 2 +- .../position_control/position_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../double_full_control_example.ino | 4 +- .../single_full_control_example.ino | 2 +- .../double_full_control_example.ino | 4 +- .../single_full_control_example.ino | 2 +- .../open_loop_position_example.ino | 2 +- .../open_loop_velocity_example.ino | 2 +- .../encoder/angle_control/angle_control.ino | 2 +- .../angle_control/angle_control.ino | 2 +- .../angle_control/angle_control.ino | 2 +- .../current_control/current_control.ino | 12 +- .../voltage_control/voltage_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../velocity_control/velocity_control.ino | 2 +- .../hall_sensor/velocity_control.ino | 2 +- .../velocity_control/velocity_control.ino | 2 +- .../full_control_serial.ino | 4 +- .../full_control_serial.ino | 4 +- .../full_control_serial.ino | 4 +- .../commander_extend_example.ino | 12 +- .../commander_no_serial.ino | 10 +- .../commander_tune_custom_loop.ino | 6 +- ...step_dir_listener_software _interrupt.ino} | 0 keywords.txt | 12 +- src/common/base_classes/FOCMotor.cpp | 47 ++-- src/common/base_classes/FOCMotor.h | 19 +- src/communication/Commander.cpp | 256 +++++++++--------- src/communication/Commander.h | 136 +++++++++- src/communication/StepDirListener.cpp | 10 +- src/communication/StepDirListener.h | 48 +++- 39 files changed, 411 insertions(+), 226 deletions(-) rename examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/{step_dir_listener_software_interrupt.ino => step_dir_listener_software _interrupt.ino} (100%) diff --git a/README.md b/README.md index 138cb4b0..fc0dc8e7 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,8 @@ Additionally, most of the efforts at this moment are still channeled towards the Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there -- 🎯 Develop a modular low-power BLDC driver board: [Arduino *SimpleFOCShield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). +- 🎯 Develop a modular *low-power BLDC (Gimbal)* driver board: [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). +- 🎯 Develop a modular *medium-power BLDC* driver board: [*Arduino Simple**FOC**PowerShield*](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). ##### NEXT RELEASE 📢: SimpleFOClibrary v2.1 > #### Implemented features in dev branch diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 64d23fd3..4eb36aa5 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -86,7 +86,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F(("Set the target angle using serial terminal:")); 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 9b42d3ec..434b2b5c 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 @@ -84,7 +84,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index c9fff7c5..746dde3d 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -39,7 +39,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(char* cmd){ command.motor(&motor, cmd); } +void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -105,7 +105,7 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onA); + command.add('A', onMotor, "motor"); Serial.println(F("Full control example: ")); Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 50962208..3e936460 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -42,7 +42,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(char* cmd){ command.motor(&motor, cmd); } +void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -106,7 +106,7 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onA); + command.add('A', onMotor, "motor"); Serial.println(F("Full control example: ")); Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index d7ef4c49..4e09d007 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -78,7 +78,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 34b677ee..2a9e991d 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -81,7 +81,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 68344fe9..372d4cd1 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -97,7 +97,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index ebb338bc..08c6f17a 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -84,7 +84,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index 8931d82a..e9f4e840 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -94,8 +94,8 @@ void setup() { motor2.target = 2; // subscribe motor to the commander - command.add('A', doMotor1); - command.add('B', doMotor2); + command.add('A', doMotor1, "motor 1"); + command.add('B', doMotor2, "motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 46a894c7..ca6a0998 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -63,7 +63,7 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('M', doMotor); + command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); 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 26055855..17b98ce5 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 @@ -109,8 +109,8 @@ void setup() { motor2.target = 2; // subscribe motor to the commander - command.add('A', doMotor1); - command.add('B', doMotor2); + command.add('A', doMotor1, "motor 1"); + command.add('B', doMotor2, "motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); 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 87064f63..4e38141f 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 @@ -70,7 +70,7 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('M', doMotor); + command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index f0a0d84b..1e728e9a 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -38,7 +38,7 @@ void setup() { motor.init(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index d0a3ea1e..0ade034f 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -40,7 +40,7 @@ void setup() { motor.init(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target velocity"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 9fdba265..3bdedcc4 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -110,7 +110,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 3e1a4085..77bc5380 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -104,7 +104,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index b3d74211..a600ed43 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -80,7 +80,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target angle using serial terminal:")); diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index f5cebc90..e4d8ba68 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -2,7 +2,6 @@ * * Torque control example using current control loop. * - * */ #include @@ -20,7 +19,7 @@ void doB(){encoder.handleB();} // current sensor InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); -// voltage set point variable +// current set point variable float target_current = 0; // instantiate the commander Commander command = Commander(Serial); @@ -41,6 +40,11 @@ void setup() { // link driver motor.linkDriver(&driver); + // current sense init hardware + current_sense.init(); + // link the current sense to the motor + motor.linkCurrentSense(¤t_sense); + // set torque mode: // TorqueControlType::current // TorqueControlType::voltage @@ -75,10 +79,10 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target current"); Serial.println(F("Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); + Serial.println(F("Set the target current using serial terminal:")); _delay(1000); } diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index cff287c7..2cac0214 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -66,7 +66,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 36330b22..89d47c78 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -68,7 +68,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index 64e0b629..b60f0018 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -58,7 +58,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target voltage using serial terminal:")); diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index 0125217d..f77023b4 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -107,7 +107,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target velocity"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 239dc233..404a7810 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -95,7 +95,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 099a190c..58d02220 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -77,7 +77,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget); + command.add('T', doTarget, "target voltage"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 5c68b29e..1f3bea03 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -56,7 +56,7 @@ void doB(){encoder.handleB();} // commander interface Commander command = Commander(Serial); -void onA(char* cmd){ command.motor(&motor, cmd); } +void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -109,7 +109,7 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onA); + command.add('A', onMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index df62b28e..e62859f0 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -62,7 +62,7 @@ PciListenerImp listenC(sensor.pinC, doC); // commander interface Commander command = Commander(Serial); -void onA(char* cmd){ command.motor(&motor, cmd); } +void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -117,7 +117,7 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onA); + command.add('A', onMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 5b7edc52..ae41499d 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -55,7 +55,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // commander interface Commander command = Commander(Serial); -void onA(char* cmd){ command.motor(&motor, cmd); } +void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -107,7 +107,7 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onA); + command.add('A', onMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); diff --git a/examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino b/examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino index 3168feda..60bbcf1f 100644 --- a/examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino +++ b/examples/utils/communication_test/commander/commander_extend_example/commander_extend_example.ino @@ -9,14 +9,12 @@ Commander command = Commander(Serial); // led control function void doLed(char* cmd){ - if(cmd[0] == CMD_SCAN ) Serial.println("led control"); - else if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); + if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); else digitalWrite(LED_BUILTIN, LOW); }; // get analog input void doAnalog(char* cmd){ - if(cmd[0] == CMD_SCAN ) Serial.println("analog read A0-A4"); - else if (cmd[0] == '0') Serial.println(analogRead(A0)); + if (cmd[0] == '0') Serial.println(analogRead(A0)); else if (cmd[0] == '1') Serial.println(analogRead(A1)); else if (cmd[0] == '2') Serial.println(analogRead(A2)); else if (cmd[0] == '3') Serial.println(analogRead(A3)); @@ -36,8 +34,8 @@ void setup() { Serial.begin(115200); // add new commands - command.add('L', doLed); - command.add('A', doAnalog); + command.add('L', doLed, "led on/off"); + command.add('A', doAnalog, "analog read A0-A4"); Serial.println(F("Commander listening")); Serial.println(F(" - Send ? to see the node list...")); @@ -51,5 +49,5 @@ void loop() { // user communication command.run(); - _delay(300); + _delay(10); } \ No newline at end of file diff --git a/examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino b/examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino index 301b3bbb..1381515e 100644 --- a/examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino +++ b/examples/utils/communication_test/commander/commander_no_serial/commander_no_serial.ino @@ -9,14 +9,12 @@ Commander command = Commander(); // led control function void doLed(char* cmd){ - if(cmd[0] == CMD_SCAN ) Serial.println("led control"); - else if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); + if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH); else digitalWrite(LED_BUILTIN, LOW); }; // get analog input void doAnalog(char* cmd){ - if(cmd[0] == CMD_SCAN ) Serial.println("analog read A0-A1"); - else if (cmd[0] == '0') Serial.println(analogRead(A0)); + if (cmd[0] == '0') Serial.println(analogRead(A0)); else if (cmd[0] == '1') Serial.println(analogRead(A1)); }; @@ -30,8 +28,8 @@ void setup() { Serial.begin(115200); // add new commands - command.add('L', doLed); - command.add('A', doAnalog); + command.add('L', doLed, "led control"); + command.add('A', doAnalog, "analog read A0-A1"); Serial.println(F("Commander running")); _delay(1000); diff --git a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino index 40643366..e8630eb5 100644 --- a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino +++ b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -54,9 +54,9 @@ void setup() { motor.initFOC(); // subscribe the new commands - command.add('C', doController); - command.add('F', doFilter); - command.add('T', doTarget); + command.add('C', doController, "tune velocity pid"); + command.add('F', doFilter, "tune velocity LPF"); + command.add('T', doTarget, "motor target"); _delay(1000); Serial.println(F("Commander listening")); diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software _interrupt.ino similarity index 100% rename from examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino rename to examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software _interrupt.ino diff --git a/keywords.txt b/keywords.txt index b32a3c89..35b42752 100644 --- a/keywords.txt +++ b/keywords.txt @@ -146,6 +146,9 @@ velocity_limit KEYWORD2 voltage_power_supply KEYWORD2 voltage_sensor_align KEYWORD2 velocity_index_search KEYWORD2 +monitor_downsample KEYWORD2 +monitor_downsample KEYWORD2 +motion_downsample KEYWORD2 pinA KEYWORD2 pinB KEYWORD2 @@ -211,4 +214,11 @@ _PI_3 LITERAL1 _SQRT3 LITERAL1 _PI LITERAL1 _HIGH_Z LITERAL1 -_HIGH_IMPEDANCE LITERAL1 \ No newline at end of file + +_MON_TARGET LITERAL1 +_MON_VOLT_Q LITERAL1 +_MON_VOLT_D LITERAL1 +_MON_CURR_Q LITERAL1 +_MON_CURR_D LITERAL1 +_MON_VEL LITERAL1 +_MON_ANGLE LITERAL1 \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index b9e4a479..99feaf46 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -86,38 +86,47 @@ void FOCMotor::monitor() { if(!monitor_port) return; bool printed = 0; - if(monitor_variables[0]){ - monitor_port->print(target); + if(monitor_variables & _MON_TARGET){ + monitor_port->print(target,4); monitor_port->print("\t"); printed= true; } - if(monitor_variables[1]) { - monitor_port->print(voltage.q); - monitor_port->print("\t"); + if(monitor_variables & _MON_VOLT_Q) { + monitor_port->print(voltage.q,4); printed= true; } - if(monitor_variables[2]) { - monitor_port->print(voltage.d); monitor_port->print("\t"); - printed= true; - } - if(monitor_variables[3]) { - monitor_port->print(current.q*1000); // mAmps + if(monitor_variables & _MON_VOLT_D) { + monitor_port->print(voltage.d,4); monitor_port->print("\t"); printed= true; } - if(monitor_variables[4]) { - monitor_port->print(current.d*1000); // mAmps - monitor_port->print("\t"); - printed= true; + // read currents if possible - even in voltage mode (if current_sense available) + if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) { + DQCurrent_s c{0,0}; + if(current_sense){ + if(torque_controller == TorqueControlType::foc_current) c = current; + else c = current_sense->getFOCCurrents(electrical_angle); + } + if(monitor_variables & _MON_CURR_Q) { + monitor_port->print(c.q*1000,2); // mAmps + monitor_port->print("\t"); + printed= true; + } + if(monitor_variables & _MON_CURR_D) { + monitor_port->print(c.d*1000,2); // mAmps + monitor_port->print("\t"); + printed= true; + } } - if(monitor_variables[5]) { - monitor_port->print(shaft_velocity); + + if(monitor_variables & _MON_VEL) { + monitor_port->print(shaft_velocity,4); monitor_port->print("\t"); printed= true; } - if(monitor_variables[6]) { - monitor_port->print(shaft_angle); + if(monitor_variables & _MON_ANGLE) { + monitor_port->print(shaft_angle,4); printed= true; } if(printed) monitor_port->println(); diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c761c164..450128b3 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -11,6 +11,16 @@ #include "../pid.h" #include "../lowpass_filter.h" + +// monitoring bitmap +#define _MON_TARGET 0b1000000 // monitor target value +#define _MON_VOLT_Q 0b0100000 // monitor voltage q value +#define _MON_VOLT_D 0b0010000 // monitor voltage d value +#define _MON_CURR_Q 0b0001000 // monitor current q value - if measured +#define _MON_CURR_D 0b0000100 // monitor current d value - if measured +#define _MON_VEL 0b0000010 // monitor velocity value +#define _MON_ANGLE 0b0000001 // monitor angle value + /** * Motiron control type */ @@ -183,9 +193,9 @@ class FOCMotor * significantly slowing the execution down!!!! */ void monitor(); - unsigned int monitor_cnt = 0 ; - unsigned int monitor_downsample = 1; - bool monitor_variables[7] = {0}; + unsigned int monitor_downsample = 10; //!< show monitor outputs each monitor_downsample calls + // initial monitoring will display target, voltage, velocity and angle + uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor /** * Sensor link: @@ -201,6 +211,9 @@ class FOCMotor // monitoring functions Print* monitor_port; //!< Serial terminal variable if provided + private: + // monitor counting variable + unsigned int monitor_cnt = 0 ; //!< counting variable }; diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index ac67e74f..97a76bb3 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -9,9 +9,10 @@ Commander::Commander(){ } -void Commander::add(char id, CommandCallback onCommand){ +void Commander::add(char id, CommandCallback onCommand, char* label ){ call_list[call_count] = onCommand; call_ids[call_count] = id; + call_label[call_count] = label; call_count++; } @@ -57,20 +58,29 @@ void Commander::run(char* user_input){ switch(id){ case CMD_SCAN: for(int i=0; i < call_count; i++){ - dump(call_ids[i], 0); - dump(":", 0); - call_list[i](cmd_scan_msg); + print(call_ids[i]); + print(":"); + if(call_label[i]) println(call_label[i]); + else println(""); } break; case CMD_VERBOSE: - if(user_input[1] != '\n') verbose = atoi(&user_input[1]); - if(verbose) dump(F("Verb. on!"),1); - else dump(F("Verb. off"),1); + if(user_input[1] != '\n') verbose = (VerboseMode)atoi(&user_input[1]); + printVerbose(F("Verb:")); + switch (verbose){ + case VerboseMode::nothing: + println(F("off!")); + break; + case VerboseMode::on_request: + case VerboseMode::user_friendly: + println(F("on!")); + break; + } break; case CMD_DECIMAL: if(user_input[1] != '\n') decimal_places = atoi(&user_input[1]); - verbosePrint(F("Decimal:")); - dump(decimal_places,1); + printVerbose(F("Decimal:")); + println(decimal_places); break; default: for(int i=0; i < call_count; i++){ @@ -84,12 +94,6 @@ void Commander::run(char* user_input){ } void Commander::motor(FOCMotor* motor, char* user_command) { - // if empty string - if( user_command[0] == CMD_SCAN ){ - dump(F("mot"),1); - return; - } - // parse command letter char cmd = user_command[0]; char sub_cmd = user_command[1]; @@ -102,30 +106,30 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ case CMD_C_Q_PID: // - verbosePrint(F("PID curr q| ")); + printVerbose(F("PID curr q| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]); else pid(&motor->PID_current_q,&user_command[1]); break; case CMD_C_D_PID: // - verbosePrint(F("PID curr d| ")); + printVerbose(F("PID curr d| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]); else pid(&motor->PID_current_d, &user_command[1]); break; case CMD_V_PID: // - verbosePrint(F("PID vel| ")); + printVerbose(F("PID vel| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]); else pid(&motor->PID_velocity, &user_command[1]); break; case CMD_A_PID: // - verbosePrint(F("PID angle| ")); + printVerbose(F("PID angle| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]); else pid(&motor->P_angle, &user_command[1]); break; case CMD_LIMITS: // - verbosePrint(F("Limits| ")); + printVerbose(F("Limits| ")); switch (sub_cmd){ case SCMD_LIM_VOLT: // voltage limit change - verbosePrint(F("volt: ")); + printVerbose(F("volt: ")); if(!GET) { motor->voltage_limit = value; motor->PID_current_d.limit = value; @@ -133,10 +137,10 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // change velocity pid limit if in voltage mode and no phase resistance set if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value; } - dump(motor->voltage_limit,1); + println(motor->voltage_limit); break; case SCMD_LIM_CURR: // current limit - verbosePrint(F("curr: ")); + printVerbose(F("curr: ")); if(!GET){ motor->current_limit = value; // if phase resistance is set, change the voltage limit as well. @@ -144,15 +148,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // 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; } - dump(motor->current_limit,1); + println(motor->current_limit); break; case SCMD_LIM_VEL: // velocity limit - verbosePrint(F("vel: ")); + printVerbose(F("vel: ")); if(!GET){ motor->velocity_limit = value; motor->P_angle.limit = value; } - dump(motor->velocity_limit,1); + println(motor->velocity_limit); break; default: printError(); @@ -160,12 +164,12 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MOTION_TYPE: - verbosePrint(F("Motion: ")); + printVerbose(F("Motion: ")); switch(sub_cmd){ case SCMD_DOWNSAMPLE: - verbosePrint(F("downsample: ")); + printVerbose(F("downsample: ")); if(!GET) motor->motion_downsample = value; - dump((int)motor->motion_downsample, 1); + println((int)motor->motion_downsample); break; default: // change control type @@ -173,19 +177,19 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->controller = (MotionControlType)value; switch(motor->controller){ case MotionControlType::torque: - dump(F("torque"),1); + println(F("torque")); break; case MotionControlType::velocity: - dump(F("vel"),1); + println(F("vel")); break; case MotionControlType::angle: - dump(F("angle"),1); + println(F("angle")); break; case MotionControlType::velocity_openloop: - dump(F("vel open"),1); + println(F("vel open")); break; case MotionControlType::angle_openloop: - dump(F("angle open"),1); + println(F("angle open")); break; } break; @@ -193,30 +197,30 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case CMD_TORQUE_TYPE: // change control type - verbosePrint(F("Torque: ")); + printVerbose(F("Torque: ")); if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ case TorqueControlType::voltage: - dump(F("volt"),1); + println(F("volt")); break; case TorqueControlType::current: - dump(F("curr"),1); + println(F("curr")); break; case TorqueControlType::foc_current: - dump(F("foc"),1); + println(F("foc")); break; } break; case CMD_STATUS: // enable/disable - verbosePrint(F("Status: ")); + printVerbose(F("Status: ")); if(!GET) (bool)value ? motor->enable() : motor->disable(); - dump(motor->enabled,1); + println(motor->enabled); break; case CMD_RESIST: // enable/disable - verbosePrint(F("R phase: ")); + printVerbose(F("R phase: ")); if(!GET){ motor->phase_resistance = value; if(motor->torque_controller==TorqueControlType::voltage){ @@ -224,22 +228,22 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->PID_velocity.limit= motor->current_limit; } } - if(_isset(motor->phase_resistance)) dump(motor->phase_resistance,1); - else dump(0,0); + if(_isset(motor->phase_resistance)) println(motor->phase_resistance); + else println(0); break; case CMD_SENSOR: // Sensor zero offset - verbosePrint(F("Sensor | ")); + printVerbose(F("Sensor | ")); switch (sub_cmd){ case SCMD_SENS_MECH_OFFSET: // zero offset - verbosePrint(F("offset: ")); + printVerbose(F("offset: ")); if(!GET) motor->sensor_offset = value; - dump(motor->sensor_offset,1); + println(motor->sensor_offset); break; case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch - verbosePrint(F("el. offset: ")); + printVerbose(F("el. offset: ")); if(!GET) motor->zero_electric_angle = value; - dump(motor->zero_electric_angle,1); + println(motor->zero_electric_angle); break; default: printError(); @@ -247,37 +251,37 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MONITOR: // get current values of the state variables - verbosePrint(F("Monitor | ")); + printVerbose(F("Monitor | ")); switch (sub_cmd){ case SCMD_GET: // get command switch((uint8_t)value){ case 0: // get target - verbosePrint(F("target: ")); - dump(motor->target,1); + printVerbose(F("target: ")); + println(motor->target); break; case 1: // get voltage q - verbosePrint(F("Vq: ")); - dump(motor->voltage.q,1); + printVerbose(F("Vq: ")); + println(motor->voltage.q); break; case 2: // get voltage d - verbosePrint(F("Vd: ")); - dump(motor->voltage.q,1); + printVerbose(F("Vd: ")); + println(motor->voltage.q); break; case 3: // get current q - verbosePrint(F("Cq: ")); - dump(motor->voltage.q,1); + printVerbose(F("Cq: ")); + println(motor->voltage.q); break; case 4: // get current d - verbosePrint(F("Cd: ")); - dump(motor->voltage.q,1); + printVerbose(F("Cd: ")); + println(motor->voltage.q); break; case 5: // get velocity - verbosePrint(F("vel: ")); - dump(motor->shaft_velocity,1); + printVerbose(F("vel: ")); + println(motor->shaft_velocity); break; case 6: // get angle - verbosePrint(F("Angle: ")); - dump(motor->shaft_angle,1); + printVerbose(F("Angle: ")); + println(motor->shaft_angle); break; default: printError(); @@ -285,20 +289,22 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case SCMD_DOWNSAMPLE: - verbosePrint(F("downsample: ")); + printVerbose(F("downsample: ")); if(!GET) motor->monitor_downsample = value; - dump((int)motor->monitor_downsample,1); + println((int)motor->monitor_downsample); break; case SCMD_CLEAR: - for(int i=0; i<7; i++) motor->monitor_variables[i] = 0; - dump(F("clear"),1); + motor->monitor_variables = (uint8_t) 0; + println(F("clear")); break; case SCMD_SET: - for(int i=0; i<7; i++){ - motor->monitor_variables[i] = user_command[value_index+i] - '0'; - dump(motor->monitor_variables[i],0); + motor->monitor_variables = (uint8_t) 0; + for(int i = 0; i < 7; i++){ + if(user_command[value_index+i] == '\n') break; + motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); + print( (user_command[value_index+i] - '0') ); } - dump("",1); + println(""); break; default: printError(); @@ -306,46 +312,42 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; default: // target change - verbosePrint(F("Target: ")); + printVerbose(F("Target: ")); motor->target = atof(user_command); - dump(motor->target,1); + println(motor->target); } } void Commander::pid(PIDController* pid, char* user_cmd){ - if( user_cmd[0] == CMD_SCAN ){ - dump(F("pid"),1); - return; - } char cmd = user_cmd[0]; bool GET = user_cmd[1] == '\n'; float value = atof(&user_cmd[1]); switch (cmd){ case SCMD_PID_P: // P gain change - verbosePrint("P: "); + printVerbose("P: "); if(!GET) pid->P = value; - dump(pid->P,1); + println(pid->P); break; case SCMD_PID_I: // I gain change - verbosePrint("I: "); + printVerbose("I: "); if(!GET) pid->I = value; - dump(pid->I,1); + println(pid->I); break; case SCMD_PID_D: // D gain change - verbosePrint("D: "); + printVerbose("D: "); if(!GET) pid->D = value; - dump(pid->D,1); + println(pid->D); break; case SCMD_PID_RAMP: // ramp change - verbosePrint("ramp: "); + printVerbose("ramp: "); if(!GET) pid->output_ramp = value; - dump(pid->output_ramp,1); + println(pid->output_ramp); break; case SCMD_PID_LIM: // limit change - verbosePrint("limit: "); + printVerbose("limit: "); if(!GET) pid->limit = value; - dump(pid->limit,1); + println(pid->limit); break; default: printError(); @@ -354,19 +356,15 @@ void Commander::pid(PIDController* pid, char* user_cmd){ } void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ - if( user_cmd[0] == CMD_SCAN ){ - dump(F("lpf"),1); - return; - } char cmd = user_cmd[0]; bool GET = user_cmd[1] == '\n'; float value = atof(&user_cmd[1]); switch (cmd){ case SCMD_LPF_TF: // Tf value change - verbosePrint(F("Tf: ")); + printVerbose(F("Tf: ")); if(!GET) lpf->Tf = value; - dump(lpf->Tf,1); + println(lpf->Tf); break; default: printError(); @@ -375,49 +373,61 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ } void Commander::variable(float* value, char* user_cmd){ - if( user_cmd[0] == CMD_SCAN ){ - dump(F("var"),1); - return; - } bool GET = user_cmd[0] == '\n'; if(!GET) *value = atof(user_cmd); - dump(*value,1); + println(*value); } -void Commander::dump(const int number, const bool newline){ - if(!com_port) return; - if(newline) com_port->println(number); - else com_port->print(number); +void Commander::print(const int number){ + if( !com_port || verbose == VerboseMode::nothing ) return; + com_port->print(number); } -void Commander::dump(const float number, const bool newline){ - if(!com_port) return; - if(newline) com_port->println((float)number,(int)decimal_places); - else com_port->print((float)number,(int)decimal_places); +void Commander::print(const float number){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->print((float)number,(int)decimal_places); } -void Commander::dump(const char* message, const bool newline){ - if(!com_port) return; - if(newline) com_port->println(message); - else com_port->print(message); +void Commander::print(const char* message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->print(message); } -void Commander::dump(const __FlashStringHelper *message, const bool newline){ - if(!com_port) return; - if(newline) com_port->println(message); - else com_port->print(message); +void Commander::print(const __FlashStringHelper *message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->print(message); } -void Commander::dump(const char message, const bool newline){ - if(!com_port) return; - if(newline) com_port->println(message); - else com_port->print(message); +void Commander::print(const char message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->print(message); +} + +void Commander::println(const int number){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->println(number); +} +void Commander::println(const float number){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->println((float)number, (int)decimal_places); +} +void Commander::println(const char* message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->println(message); +} +void Commander::println(const __FlashStringHelper *message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->println(message); +} +void Commander::println(const char message){ + if(!com_port || verbose == VerboseMode::nothing ) return; + com_port->println(message); } -void Commander::verbosePrint(const char* message){ - if(verbose) dump(message,0); +void Commander::printVerbose(const char* message){ + if(verbose == VerboseMode::user_friendly) print(message); } -void Commander::verbosePrint(const __FlashStringHelper *message){ - if(verbose) dump(message,0); +void Commander::printVerbose(const __FlashStringHelper *message){ + if(verbose == VerboseMode::user_friendly) print(message); } void Commander::printError(){ - dump(F("err"), 1); + print(F("err")); } \ No newline at end of file diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 4e376793..03657094 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -7,9 +7,29 @@ #include "../common/lowpass_filter.h" #include "commands.h" +// Commander verbose display to the user type +enum VerboseMode{ + nothing = 0, // display nothing - good for monitoring + on_request, // display only on user request + user_friendly // display textual messages to the user +}; + + // callback function pointer definiton typedef void (* CommandCallback)(char*); //!< command callback function pointer +/** + * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`) + * + * - This class can be used in combination with HardwareSerial instance which it would read and write + * or it can be used to parse strings that have been received from the user outside this library + * - Commander class implements command protocol for few standard components of the SimpleFOC library + * - FOCMotor + * - PIDController + * - LowPassFilter + * - Commander also provides a very simple command > callback interface that enables user to + * attach a callback function to certain command id - see function add() + */ class Commander { public: @@ -25,13 +45,34 @@ class Commander /** * Function reading the serial port and firing callbacks that have been added to the commander * once the user has requested them - when he sends the command + * + * - It has default commands (the letters can be changed in the commands.h file) + * '@' - Verbose mode + * '#' - Number of decimal places + * '?' - Scan command - displays all the labels of attached nodes */ void run(); + /** + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * + * - It has default commands (the letters can be changed in the commands.h file) + * '@' - Verbose mode + * '#' - Number of decimal places + * '?' - Scan command - displays all the labels of attached nodes + * + * @param reader - HardwareSerial to read user input + */ void run(HardwareSerial &reader); - /**x + /** * Function reading the string of user input and firing callbacks that have been added to the commander * once the user has requested them - when he sends the command * + * - It has default commands (the letters can be changed in the commands.h file) + * '@' - Verbose mode + * '#' - Number of decimal places + * '?' - Scan command - displays all the labels of attached nodes + * * @param user_input - string of user inputs */ void run(char* user_input); @@ -40,56 +81,129 @@ class Commander * Function adding a callback to the coomander withe the command id * @param id - char command letter * @param onCommand - function pointer void function(char*) + * @param label - string label to be displayed when scan command sent */ - void add(char id , CommandCallback onCommand); + void add(char id , CommandCallback onCommand, char* label = nullptr); // printing variables - bool verbose = 1; //!< flag signaling that the commands should output user understanable text + VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers // monitoring functions HardwareSerial* com_port = nullptr; //!< Serial terminal variable if provided /** + * + * FOC motor (StepperMotor and BLDCMotor) command interface + * - It has several paramters (the letters can be changed in the commands.h file) + * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) + * 'D' - D current PID controller & LPF (see function pid and lpf for commands) + * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) + * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) + * 'L' - Limits + * sub-commands: + * 'C' - Current + * 'U' - Voltage + * 'V' - Velocity + * 'C' - Motion control type config + * sub-commands: + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type + * sub-commands: + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) + * sub-commands: + * '0' - enable + * '1' - disable + * 'R' - Motor resistance + * 'S' - Sensor offsets + * sub-commands: + * 'M' - sensor offset + * 'E' - sensor electrical zero + * 'M' - Monitoring control + * sub-commands: + * 'D' - downsample monitoring + * 'C' - clear monitor + * 'S' - set monitoring variables + * 'G' - get variable value + * '' - Target get/set + * + * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) + * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5) + * */ void motor(FOCMotor* motor, char* user_cmd); + + /** + * Low pass fileter command interface + * - It only has one property - filtering time constant Tf + * - It can be get by sending 'F' + * - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01) + */ void lpf(LowPassFilter* lpf, char* user_cmd); + /** + * PID controller command interface + * - It has several paramters (the letters can be changed in the commands.h file) + * - P gain - 'P' + * - I gain - 'I' + * - D gain - 'D' + * - output ramp - 'R' + * - output limit - 'L' + * - Each of them can be get by sening the command letter -(ex. 'D' - to get the D gain) + * - Each of them can be set by sending 'IDvalue' - (ex. I1.5 for setting I=1.5) + */ void pid(PIDController* pid, char* user_cmd); + /** + * Float variable command interface + * - It only has one property - one float value + * - It can be get by sending an empty string '\n' + * - It can be set by sending 'value' - (ex. 0.01 for settin *value=0.01) + */ void variable(float* value, char* user_cmd); private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number char call_ids[20]; //!< added callback commands + char* call_label[20]; //!< added callback labels 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 int rec_cnt = 0; //!< number of characters receives - char cmd_scan_msg[2] = {CMD_SCAN,0}; //!< scan message to be sent to the nodes - should be solved differently maybe // serial printing functions /** * print the string message only if verbose mode on * @param message - message to be printed */ - void verbosePrint(const char* message); + void printVerbose(const char* message); /** * Print the string message only if verbose mode on * - Function handling the case for strings defined by F macro * @param message - message to be printed */ - void verbosePrint(const __FlashStringHelper *message); + void printVerbose(const __FlashStringHelper *message); /** * print the numbers to the serial with desired decimal point number * @param message - number to be printed * @param newline - if needs lewline (1) otherwise (0) */ - void dump(const float number, const bool newline = 1); - void dump(const int number, const bool newline = 1); - void dump(const char* message, const bool newline = 1); - void dump(const __FlashStringHelper *message, const bool newline = 1); - void dump(const char message, const bool newline = 1); + void print(const float number); + void print(const int number); + void print(const char* message); + void print(const __FlashStringHelper *message); + void print(const char message); + void println(const float number); + void println(const int number); + void println(const char* message); + void println(const __FlashStringHelper *message); + void println(const char message); void printError(); }; diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index 3bf9d3fe..34a541aa 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -1,12 +1,9 @@ #include "StepDirListener.h" StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _step_per_rotation){ - pin_step = _pinStep; pin_dir = _pinDir; - step_per_rotation = _step_per_rotation; - } void StepDirListener::init(){ @@ -23,8 +20,10 @@ void StepDirListener::attach(float* variable){ attached_variable = variable; } -void StepDirListener::handle(){ +void StepDirListener::handle(){ + // read step status bool step = digitalRead(pin_step); + // update counter only on rising edge if(step && step != step_active){ if(digitalRead(pin_dir)) count++; @@ -32,9 +31,10 @@ void StepDirListener::handle(){ count--; } step_active = step; + // if attached variable update it if(attached_variable) *attached_variable = getValue(); } - +// calculate the position from counter float StepDirListener::getValue(){ return (float) count / step_per_rotation * _2PI; } \ No newline at end of file diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 0eff2bbc..7674600c 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -4,26 +4,54 @@ #include "Arduino.h" #include "../common/foc_utils.h" +/** + * Step/Dir listenner class for easier interraction with this communication interface. + */ class StepDirListener { public: - StepDirListener(int pinStep, int pinDir, float step_per_rotation); - void enableInterrupt(void (*doA)()); + + /** + * Constructor for step/direction interface + * @param step - pin + * @param direction - pin + * @param step_per_rotation - number of steps per motor rotation + */ + StepDirListener(int pinStep, int pinDir, float step_per_rotation = 1); + /** + * Start listenning for step commands + * + * @param handleStep - on step received handler + */ + void enableInterrupt(void (*handleStep)()); + + /** + * Initialise dir and step commands + */ void init(); + /** + * step handler + */ void handle(); - + /** + * Get so far received valued + */ float getValue(); - + /** + * Attach the value to be updated on each step receive + * - no need to call getValue function + */ void attach(float* variable); - int pin_step; - int pin_dir; - long count; + // variables + int pin_step; //!< step pin + int pin_dir; //!< direction pin + long count; //!< current counter value - should be set to 0 for homing private: - float* attached_variable = nullptr; - long step_per_rotation; - bool step_active = 0; + float* attached_variable = nullptr; //!< pointer to the attached variable + long step_per_rotation; //!< number of steps per rotation + bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable }; #endif \ No newline at end of file From d23d1dff909573f6f4e86f1ff2a522d5c7fb1b48 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 23 Feb 2021 16:22:55 +0100 Subject: [PATCH 110/749] fix example --- README.md | 5 +++-- ...nterrupt.ino => step_dir_listener_software_interrupt.ino} | 0 2 files changed, 3 insertions(+), 2 deletions(-) rename examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/{step_dir_listener_software _interrupt.ino => step_dir_listener_software_interrupt.ino} (100%) diff --git a/README.md b/README.md index fc0dc8e7..d826f783 100644 --- a/README.md +++ b/README.md @@ -11,8 +11,9 @@ Additionally, most of the efforts at this moment are still channeled towards the Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there -- 🎯 Develop a modular *low-power BLDC (Gimbal)* driver board: [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). -- 🎯 Develop a modular *medium-power BLDC* driver board: [*Arduino Simple**FOC**PowerShield*](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). +- 🎯 Develop a modular FOC supporting BLDC driver boards: + - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). + - ***NEW***: *Medium-power* BLDC driver (<50Amps): [*Arduino Simple**FOC**PowerShield*](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). ##### NEXT RELEASE 📢: SimpleFOClibrary v2.1 > #### Implemented features in dev branch diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software _interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino similarity index 100% rename from examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software _interrupt.ino rename to examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino From 5201e8d1c136fe6d18358a14012256fdb2f867de Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 23 Feb 2021 23:34:54 +0100 Subject: [PATCH 111/749] SAMD21 uses USBSerial, which subclasses Stream, but not HardwareSerial --- src/communication/Commander.cpp | 4 ++-- src/communication/Commander.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 97a76bb3..ed7108f8 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -1,7 +1,7 @@ #include "Commander.h" -Commander::Commander(HardwareSerial& serial){ +Commander::Commander(Stream& serial){ com_port = &serial; } Commander::Commander(){ @@ -430,4 +430,4 @@ void Commander::printVerbose(const __FlashStringHelper *message){ } void Commander::printError(){ print(F("err")); -} \ No newline at end of file +} diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 03657094..f0228dcf 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -39,7 +39,7 @@ class Commander * * @param serial - Serial com port instance */ - Commander(HardwareSerial &serial); + Commander(Stream &serial); Commander(); /** @@ -90,7 +90,7 @@ class Commander uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers // monitoring functions - HardwareSerial* com_port = nullptr; //!< Serial terminal variable if provided + Stream* com_port = nullptr; //!< Serial terminal variable if provided /** * From 32a164b2e30b915640ba8e0e1ac618e8c8c67cd1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 23 Feb 2021 23:37:42 +0100 Subject: [PATCH 112/749] inital SAMD21 support, with example, tested 3-PWM and Hardware 6-PWM --- .../SAMD_examples/README.md | 62 ++ .../nano33IoT_velocity_control.ino | 65 ++ src/drivers/hardware_specific/generic_mcu.cpp | 4 +- src/drivers/hardware_specific/samd21_mcu.cpp | 965 ++++++++++++++++++ .../samd21_wo_associations.h | 131 +++ src/drivers/hardware_specific/samd_debug.h | 107 ++ 6 files changed, 1333 insertions(+), 1 deletion(-) create mode 100644 examples/hardware_specific_examples/SAMD_examples/README.md create mode 100644 examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino create mode 100644 src/drivers/hardware_specific/samd21_mcu.cpp create mode 100644 src/drivers/hardware_specific/samd21_wo_associations.h create mode 100644 src/drivers/hardware_specific/samd_debug.h diff --git a/examples/hardware_specific_examples/SAMD_examples/README.md b/examples/hardware_specific_examples/SAMD_examples/README.md new file mode 100644 index 00000000..d1a52584 --- /dev/null +++ b/examples/hardware_specific_examples/SAMD_examples/README.md @@ -0,0 +1,62 @@ + +# SAMD Support + +SimpleFOC supports many SAMD21 MCUs, really any SAMD21 supported by Arduino core should work. + +## Pin assignments + +The SAMD chips have some very powerful PWM features, but do not have flexible pin assignments. + +You should be able to use *most* (but not all!), pin combinations for attaching your motor's PWM pins. Please ignore the board descriptions and pinout diagrammes regarding PWM-pins on SAMD boards. They are pretty much all incorrect to varying degrees of awfulness. + +On SAMD we use TCC and TC timer peripherals (built into the SAMD chip) to control the PWM. Depending on the chip there are various timer units, whose PWM outputs are attached to various different pins, and it is all very complicated. Luckily SimpleFOC sets it all up automatically *if* there is a compatible configuration for those pins. + +Not all timers are created equal. The TCC timers are pretty awesome for PWM motor control, while the TC timers are just ok for the job. So to get best performance, you want to use just TCC timer pins if you can. + +By enabling + +``` + #define SIMPLEFOC_SAMD_DEBUG +``` + +in drivers/hardware_specific/samd_mcu.cpp
    +you will see a table of pin assignments printed on the serial console, as well as the timers SimpleFOC was able to find and configure on the pins you specified. You can use this to optimize your choice of pins if you want. + +You can configure up to 12 pins for PWM motor control, i.e. 6x 2-PWM motors, 4x 3-PWM motors, 3x 4-PWM motors or 2x 6-PWM motors. + +## PWM control modes + +All modes (3-PWM, 6-PWM, Stepper 2-PWM and Stepper 4-PWM) are supported. + +For 2-, 3- amd 4- PWM, any valid pin-combinations can be used. If you stick to TCC timers rather than using TC timers, then you'll get getter PWM waveforms. If you use pins which are all on the same TCC unit, you'll get the best result, with the PWM signals all perfectly aligned as well. + +For 6-PWM, the situation is much more complicated:
    +TC timers cannot be used for 6-PWM, only TCC timers. + +For Hardware Dead-Time insertion, you must use H and L pins for one phase from the same TCC unit, and on the same channel, but using complementary WOs (Waveform Outputs, i.e. PWM output pins). Check the table to find pins on the same channel (like TCC0-0) but complementary WOs (like TCC0-0[0] and TCC0-0[4] or TCC1-0[0] and TCC1-0[2]). + +For Software Dead-Time insertion, you must use the same TCC and different channels for the H and L pins of the same phase. + +Note: in all of the above note that you *cannot* set the timers or WOs used - they are fixed, and determined by the pins you selected. SimpleFOC will find the best combination of timers given the pins, trying to use TCC timers before TC, and trying to keep things on the same timers as much as possible. If you configure multiple motors, it will take into account the pins already assigned to other motors. +So it is matter of choosing the right pins, nothing else. + +Note also: Unfortunately you can't set the PWM frequency. It is currently fixed at 24KHz. This is a tradeoff between limiting PWM resolution vs +increasing frequency, and also due to keeping the pin assignemts flexible, which would not be possible if we ran the timers at different rates. + +## Status + +Currently, SAMD21 is supported, and SAMD51 is unsupported. SAMD51 support is in progress. + +Boards tested: + + * Arduino Nano 33 IoT + * Arduino MKR1000 + * Arduino MKR1010 Wifi + * Seeduino XIAO + * Feather M0 Basic + +Environments tested: + + * Arduino IDE + * Arduino Pro IDE + * Sloeber diff --git a/examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino new file mode 100644 index 00000000..a7fef55e --- /dev/null +++ b/examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino @@ -0,0 +1,65 @@ + +// show the infos for SAMD pin assignment on serial console +// set this #define SIMPLEFOC_SAMD_DEBUG in drivers/hardware_specific/samd21_mcu.h + + +#include "Arduino.h" +#include +#include +#include + +// this is for an AS5048B absolute magnetic encoder on I2C address 0x41 +MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8); + +// small BLDC gimbal motor, 7 pole-pairs +BLDCMotor motor = BLDCMotor(7); +// 3-PWM driving on pins 6, 5 and 8 - these are all on the same timer unit (TCC0), but different channels +BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); + +// velocity set point variable +float target_velocity = 2.0; +// instantiate the commander +Commander command = Commander(SerialUSB); +void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } + + +void setup() { + Serial.begin(115200); + delay(1000); + Serial.println("Initializing..."); + + sensor.init(); + Wire.setClock(400000); + motor.linkSensor(&sensor); + driver.voltage_power_supply = 9; + driver.init(); + motor.linkDriver(&driver); + motor.controller = MotionControlType::velocity; + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0.001; + motor.PID_velocity.output_ramp = 1000; + motor.LPF_velocity.Tf = 0.01; + motor.voltage_limit = 9; + //motor.P_angle.P = 20; + motor.init(); + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target velocity"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + delay(100); +} + + + +void loop() { +// Serial.print("Sensor: "); +// Serial.println(sensor.getAngle()); + motor.loopFOC(); + motor.move(target_velocity); + // user communication + command.run(); +} diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index d55b9ff3..73aa3c9f 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -12,6 +12,8 @@ #elif defined(_STM32_DEF_) // or stm32 +#elif defined(_SAMD21_) // samd21 for the moment, samd51 in progress... + #else // function setting the high pwm frequency to the supplied pins @@ -86,4 +88,4 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i } -#endif \ No newline at end of file +#endif diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..cdbd82f8 --- /dev/null +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,965 @@ + + +#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 + + + +#ifndef SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS +#define SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS false +#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 +#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); +} + + + +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; +}; + +#ifdef SIMPLEFOC_SAMD_DEBUG +#include "./samd_debug.h" +#endif + + + + +/** + * 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 + * 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;iSTATUS.bit.SYNCBUSY); // Wait for synchronization + + REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC | // Set the duty cycle to 50/50 HIGH/LOW + GCLK_GENCTRL_GENEN | // Enable GCLK4 + GCLK_GENCTRL_SRC_DFLL48M | // Set the 48MHz clock source + GCLK_GENCTRL_ID(4); // Select GCLK4 + while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + +#ifdef SIMPLEFOC_SAMD_DEBUG + Serial.println("Configured clock..."); +#endif + } +} + + + + +/** + * Configure a TCC unit + * pwm_frequency is fixed at 24kHz for now. We could go slower, but going + * faster won't be possible without sacrificing resolution. + */ +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1) { + // TODO for the moment we ignore the frequency... + if (!tccConfigured[tccConfig.tcc.tccn]) { + uint32_t GCLK_CLKCTRL_ID_ofthistcc = -1; + switch (tccConfig.tcc.tccn>>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; + } + + // 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 + + 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 ); + // 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 + } + else { + Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo); + + uint8_t invenMask = ~(1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; + syncTCC(tcc); // wait for sync + + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH; // Set wave form configuration + while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // 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 + } + + 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% + uint8_t chanCount = (tccConfig.tcc.tccn==1||tccConfig.tcc.tccn==2)?2:4; + 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 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 + +#ifdef SIMPLEFOC_SAMD_DEBUG + Serial.print(" Initialized TCC "); + Serial.print(tccConfig.tcc.tccn); + Serial.print("-"); + Serial.print(tccConfig.tcc.chan); + Serial.print("["); + Serial.print(tccConfig.wo); + Serial.println("]"); +#endif + } + } + else if (tccConfig.tcc.tccnCTRLA.bit.ENABLE = 0; + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); + + 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 + } + + tcc->CTRLA.bit.ENABLE = 1; + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); + +#ifdef SIMPLEFOC_SAMD_DEBUG + Serial.print("(Re-)Initialized TCC "); + Serial.print(tccConfig.tcc.tccn); + Serial.print("-"); + Serial.print(tccConfig.tcc.chan); + Serial.print("["); + Serial.print(tccConfig.wo); + Serial.println("]"); +#endif + } + + +} + + + + + +/** + * 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 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 ); + } + 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 ); + } +} + + + + + + + +/** + * 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 new file mode 100644 index 00000000..3ab087e0 --- /dev/null +++ b/src/drivers/hardware_specific/samd21_wo_associations.h @@ -0,0 +1,131 @@ + + +struct wo_association { + EPortType port; + uint32_t pin; + ETCChannel tccE; + uint8_t woE; + ETCChannel tccF; + uint8_t woF; +}; + + + +#ifdef _SAMD21_ + + + +#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;iCTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC; +//while (TCC0->SYNCBUSY.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"); +} + + + From 7a3b2a07206c367ee37c831835ea715d1f289fe6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 23 Feb 2021 23:56:37 +0100 Subject: [PATCH 113/749] Comprehensive OSC example on ESP32 --- examples/osc_control_examples/README.md | 28 ++ .../osc_esp32_fullcontrol/osc_fullcontrol.pd | 384 ++++++++++++++++++ .../simplefoc_osc_esp32_fullcontrol.ino | 351 ++++++++++++++++ .../osc_esp32_fullcontrol/ssid.h_rename_me | 4 + .../osc_fullcontrol_screenshot.png | Bin 0 -> 261643 bytes 5 files changed, 767 insertions(+) create mode 100644 examples/osc_control_examples/README.md create mode 100644 examples/osc_control_examples/osc_esp32_fullcontrol/osc_fullcontrol.pd create mode 100644 examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino create mode 100644 examples/osc_control_examples/osc_esp32_fullcontrol/ssid.h_rename_me create mode 100644 examples/osc_control_examples/osc_fullcontrol_screenshot.png diff --git a/examples/osc_control_examples/README.md b/examples/osc_control_examples/README.md new file mode 100644 index 00000000..bd6f7766 --- /dev/null +++ b/examples/osc_control_examples/README.md @@ -0,0 +1,28 @@ + +# OSC control examples + +OSC - opensoundcontrol.org is a simple message exchange protocol. Widely used in the Audio world it is being hailed as the successor to MIDI. But MIDI itself, and now OSC, has always been about sending and receiving fairly simple control messages, at medium speed, and that makes it well suited for the same kind of task in robotics or other control applications. + +As a protocol it is simple, making implementation quite easy, and while binary, simple enough to be fairly "human readable", which aids in debugging and reduces errors. + +The main advantage of using it is that there is a bunch of ready made software, and also libraries to use in your own programs, neaning you don't have to re-invent these things from scratch. + +## TouchOSC + +The first example shows how to set up control of a motor from TouchOSC. It's a super-fast way to set up a customizable GUI for your motor-project, that runs on your phone... + +## purr-Data + +The second example, "simplefoc\_osc\_esp32\_fullcontrol.ino" allows setting the variables and tuning the motor parameters, in the same way as the serial control but via OSC. The file "osc\_fullcontrol.pd" contains a sample GUI to go with that sketch, allowing the control and tuning of 2 BLDC motors. + +Here a screenshot of what it looks like in purr-Data: + +![Screenshot from pD](osc_fullcontrol_screenshot.png?raw=true "pD controlling 2 BLDC motors") + + +## Links to software used in examples + +- OSC - opensoundcontrol.org +- pD - https://puredata.info +- TouchOSC - https://hexler.net/products/touchosc + diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_fullcontrol.pd b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_fullcontrol.pd new file mode 100644 index 00000000..ddbf2f99 --- /dev/null +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_fullcontrol.pd @@ -0,0 +1,384 @@ +#N struct text float x float y text t; +#N canvas 1842 146 1519 1052 12; +#X obj 501 697 cnv 15 193 209 empty empty Tuning\ M1 20 12 0 14 #e0e0e0 +#404040 0; +#X obj 787 477 mrpeach/unpackOSC; +#X obj 132 586 print oscin; +#X obj 787 504 print oscout; +#X obj 723 449 spigot; +#X obj 774 452 tgl 15 1 empty empty Debug 17 7 0 10 #fcfcfc #000000 +#000000 1 1; +#X msg 591 503 disconnect; +#X obj 132 558 spigot; +#X obj 114 562 tgl 15 1 empty empty Debug -34 6 0 10 #fcfcfc #000000 +#000000 0 1; +#X obj 132 531 mrpeach/unpackOSC; +#X obj 673 477 mrpeach/udpsend; +#X obj 132 496 mrpeach/udpreceive 8000; +#X obj 673 422 mrpeach/packOSC; +#X obj 1043 150 hsl 249 25 -5000 5000 0 0 empty empty Set\ Point\ M1 +-2 -8 0 10 #fcfcfc #000000 #000000 0 1; +#X obj 1044 197 hsl 247 25 -15 15 0 0 empty empty Set\ Point\ M2 -2 +-8 0 10 #fcfcfc #000000 #000000 12300 1; +#X obj 120 153 bng 53 250 50 0 empty empty STOP 14 26 0 10 #fc1204 +#000000 #ffffff; +#X obj 200 102 * 0.10472; +#X obj 202 169 hsl 235 30 -520 520 0 0 empty empty Set\ point\ (Velocity) +-2 -8 0 10 #fcfcfc #000000 #000000 11500 1; +#X obj 673 449 spigot; +#X obj 653 452 tgl 15 1 empty empty Enable\ send -71 6 0 10 #fcfcfc +#000000 #000000 1 1; +#X msg 484 478 connect 192.168.1.43 8000; +#X obj 673 395 speedlim 100; +#X obj 231 573 mrpeach/routeOSC /M1 /M2; +#X obj 231 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L +/C; +#X obj 326 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc +#000000 #000000 0 1; +#X obj 326 812 % 6.28319; +#X obj 326 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc +#000000 #000000 0.137548 256 3; +#X obj 113 804 nbx 7 27 -1e+37 1e+37 0 0 empty empty Set\ point 0 -15 +0 18 #fcfcfc #000000 #000000 0 256 3; +#X obj 326 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 0 +0 18 #fcfcfc #000000 #000000 -348.637 256 3; +#X obj 326 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 0 +0 18 #fcfcfc #000000 #000000 0.0649328 256 3; +#X obj 538 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 0.2 256 3; +#X obj 537 776 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 20 256 3; +#X obj 538 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 0 +10 #fcfcfc #000000 #000000 0.0001 256 3; +#X obj 539 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 0 +10 #fcfcfc #000000 #000000 1000 256 3; +#X obj 539 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 +0 10 #fcfcfc #000000 #000000 0.01 256 3; +#X obj 605 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 608 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 609 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 +0 10 #fcfcfc #000000 #000000 8 256 3; +#X obj 122 278 hradio 53 0 1 3 motorselect empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 0; +#X scalar text 172 305 \; \;; +#X obj 122 372 select 0 1 2; +#X msg 122 399 prefix /M?; +#X msg 149 423 prefix /M1; +#X msg 178 445 prefix /M2; +#X obj 789 422 mrpeach/packOSC; +#X obj 789 395 speedlim 100; +#X msg 592 531 typetags \$1; +#X obj 571 535 tgl 15 1 empty empty OSC\ type\ tags -80 7 0 10 #ffffff +#000000 #000000 1 1; +#X text 63 286 Choose Motor, f 7; +#X text 137 339 All, f 4; +#X text 191 339 M1, f 4; +#X text 247 339 M2, f 4; +#X text 152 77 RPM; +#X obj 1008 148 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 +#000000 #ffffff; +#X obj 1009 195 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204 +#000000 #ffffff; +#X obj 74 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc #000000 +#000000 1; +#X text 8 711 Control; +#X text 67 752 Voltage; +#X text 124 753 Velocity; +#X text 189 754 Position; +#X obj 312 101 /; +#X obj 312 129 * 6.28319; +#X text 424 75 cm, f 4; +#X text 393 51 Wheel diameter; +#X obj 394 100 * 0.0314159; +#X msg 348 636 set \$1; +#X msg 376 637 set \$1; +#X msg 407 636 set \$1; +#X msg 435 636 set \$1; +#X msg 466 636 set \$1; +#X msg 495 636 set \$1; +#X msg 524 637 set \$1; +#X msg 554 637 set \$1; +#X msg 583 637 set \$1; +#X obj 75 898 s osctargetedout; +#X obj 75 866 prepend /M1/C; +#X obj 773 304 r osctargetedout; +#X obj 593 912 prepend /M1/K; +#X obj 602 925 prepend /M1/N; +#X obj 609 936 prepend /M1/L; +#X obj 564 976 s osctargetedout; +#X obj 1271 697 cnv 15 193 209 empty empty Tuning\ M2 20 12 0 14 #e0e0e0 +#404040 0; +#X obj 1001 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L +/C; +#X obj 1096 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc +#000000 #000000 0 1; +#X obj 1096 812 % 6.28319; +#X obj 1096 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc +#000000 #000000 -0.13018 256 3; +#X obj 883 804 nbx 7 27 -1e+37 1e+37 0 0 setpointin2 empty Set\ point +0 -15 0 18 #fcfcfc #000000 #000000 0 256 3; +#X obj 1096 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 +0 0 18 #fcfcfc #000000 #000000 -346.273 256 3; +#X obj 1096 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 +0 0 18 #fcfcfc #000000 #000000 0.0657255 256 3; +#X obj 1308 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 0.2 256 3; +#X obj 1308 778 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1308 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 +0 10 #fcfcfc #000000 #000000 0.001 256 3; +#X obj 1309 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 +0 10 #fcfcfc #000000 #000000 1000 256 3; +#X obj 1309 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8 +0 10 #fcfcfc #000000 #000000 0.01 256 3; +#X obj 1375 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 +-8 0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1378 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8 +0 10 #fcfcfc #000000 #000000 20 256 3; +#X obj 1379 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8 +0 10 #fcfcfc #000000 #000000 8 256 3; +#X obj 844 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 1; +#X text 778 711 Control; +#X text 837 752 Voltage; +#X text 894 753 Velocity; +#X text 959 754 Position; +#X msg 1118 636 set \$1; +#X msg 1146 637 set \$1; +#X msg 1177 636 set \$1; +#X msg 1205 636 set \$1; +#X msg 1236 636 set \$1; +#X msg 1265 636 set \$1; +#X msg 1294 637 set \$1; +#X msg 1324 637 set \$1; +#X msg 1353 637 set \$1; +#X obj 845 898 s osctargetedout; +#X obj 1325 976 s osctargetedout; +#X obj 1379 936 prepend /M2/L; +#X obj 1372 925 prepend /M2/N; +#X obj 1364 912 prepend /M2/K; +#X obj 1296 947 prepend /M2/F; +#X obj 1291 940 prepend /M2/R; +#X obj 1287 933 prepend /M2/D; +#X obj 1281 925 prepend /M2/I; +#X obj 1276 917 prepend /M2/P; +#X obj 526 947 prepend /M1/F; +#X obj 521 940 prepend /M1/R; +#X obj 517 933 prepend /M1/D; +#X obj 511 925 prepend /M1/I; +#X obj 506 917 prepend /M1/P; +#X obj 393 78 nbx 2 14 0 50 0 1 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 6 256 2; +#X obj 299 71 nbx 5 24 -20 20 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 -0.266666 256 2; +#X obj 179 74 nbx 7 23 -5000 5000 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 -84.8824 256 2; +#X obj 577 169 hsl 104 30 -3.1415 3.1415 0 0 empty empty Set\ point\ (Position) +-2 -8 0 10 #fcfcfc #000000 #000000 10300 1; +#X obj 708 68 vsl 31 122 0 12 0 0 empty empty Set\ point\ (Voltage) +0 -9 0 10 #fcfcfc #000000 #000000 0 1; +#X obj 246 13 / 0.10472; +#X msg 318 13 set \$1; +#X obj 457 11 *; +#X obj 384 11 / 6.28319; +#X obj 547 96 /; +#X obj 547 125 * 6.28319; +#X obj 547 66 nbx 5 24 -100 100 0 0 empty empty empty 0 -8 0 10 #fcfcfc +#000000 #000000 0.0599999 256 2; +#X text 534 68 m; +#X obj 20 6 r setpointin; +#X obj 17 36 r setpointin2; +#X obj 120 6 spigot; +#X obj 120 42 spigot; +#X obj 16 63 r motorselect; +#X obj 1339 99 r setpointin; +#X obj 1345 150 r setpointin2; +#X msg 493 11 set \$1; +#X obj 22 103 > 1; +#X obj 59 103 <= 1; +#X msg 1110 529 /M?/params; +#X obj 1110 502 loadbang; +#X msg 1339 126 set \$1; +#X msg 1343 174 set \$1; +#X obj 639 9 *; +#X obj 566 9 / 6.28319; +#X msg 675 9 set \$1; +#X obj 483 448 loadbang; +#X text 284 74 m/s; +#X obj 845 866 prepend /M2/C; +#X msg 163 226 sendtyped /M?/t f 0; +#X obj 458 224 prepend sendtyped /t f; +#X msg 991 301 sendtyped /M2/t f 0; +#X msg 983 274 sendtyped /M1/t f 0; +#X obj 1051 286 prepend sendtyped /M1/t f; +#X obj 1047 322 prepend sendtyped /M2/t f; +#X obj 475 171 nbx 7 21 -1e+37 1e+37 0 0 empty empty rad 0 -8 0 10 +#fcfcfc #000000 #000000 2 256 2; +#X connect 1 0 3 0; +#X connect 4 0 1 0; +#X connect 5 0 4 1; +#X connect 6 0 10 0; +#X connect 7 0 2 0; +#X connect 8 0 7 1; +#X connect 9 0 7 0; +#X connect 9 0 22 0; +#X connect 11 0 9 0; +#X connect 12 0 18 0; +#X connect 12 0 4 0; +#X connect 13 0 163 0; +#X connect 14 0 164 0; +#X connect 15 0 159 0; +#X connect 16 0 17 0; +#X connect 17 0 131 0; +#X connect 17 0 134 0; +#X connect 17 0 160 0; +#X connect 18 0 10 0; +#X connect 19 0 18 1; +#X connect 20 0 10 0; +#X connect 21 0 12 0; +#X connect 22 0 23 0; +#X connect 22 1 82 0; +#X connect 23 0 26 0; +#X connect 23 1 29 0; +#X connect 23 2 28 0; +#X connect 23 3 27 0; +#X connect 23 4 65 0; +#X connect 23 5 66 0; +#X connect 23 6 67 0; +#X connect 23 7 68 0; +#X connect 23 8 69 0; +#X connect 23 9 70 0; +#X connect 23 10 71 0; +#X connect 23 11 72 0; +#X connect 23 12 73 0; +#X connect 25 0 24 0; +#X connect 28 0 25 0; +#X connect 30 0 125 0; +#X connect 31 0 124 0; +#X connect 32 0 123 0; +#X connect 33 0 122 0; +#X connect 34 0 121 0; +#X connect 35 0 77 0; +#X connect 36 0 78 0; +#X connect 37 0 79 0; +#X connect 38 0 40 0; +#X connect 40 0 41 0; +#X connect 40 1 42 0; +#X connect 40 2 43 0; +#X connect 41 0 12 0; +#X connect 42 0 12 0; +#X connect 43 0 12 0; +#X connect 44 0 18 0; +#X connect 44 0 4 0; +#X connect 45 0 44 0; +#X connect 46 0 12 0; +#X connect 47 0 46 0; +#X connect 53 0 162 0; +#X connect 54 0 161 0; +#X connect 55 0 75 0; +#X connect 60 0 61 0; +#X connect 61 0 17 0; +#X connect 64 0 60 1; +#X connect 64 0 133 1; +#X connect 64 0 135 1; +#X connect 64 0 153 1; +#X connect 65 0 30 0; +#X connect 66 0 31 0; +#X connect 67 0 32 0; +#X connect 68 0 33 0; +#X connect 69 0 34 0; +#X connect 70 0 35 0; +#X connect 71 0 36 0; +#X connect 72 0 37 0; +#X connect 73 0 55 0; +#X connect 75 0 74 0; +#X connect 76 0 45 0; +#X connect 77 0 80 0; +#X connect 78 0 80 0; +#X connect 79 0 80 0; +#X connect 82 0 85 0; +#X connect 82 1 88 0; +#X connect 82 2 87 0; +#X connect 82 3 86 0; +#X connect 82 4 102 0; +#X connect 82 5 103 0; +#X connect 82 6 104 0; +#X connect 82 7 105 0; +#X connect 82 8 106 0; +#X connect 82 9 107 0; +#X connect 82 10 108 0; +#X connect 82 11 109 0; +#X connect 82 12 110 0; +#X connect 84 0 83 0; +#X connect 87 0 84 0; +#X connect 89 0 120 0; +#X connect 90 0 119 0; +#X connect 91 0 118 0; +#X connect 92 0 117 0; +#X connect 93 0 116 0; +#X connect 94 0 115 0; +#X connect 95 0 114 0; +#X connect 96 0 113 0; +#X connect 97 0 158 0; +#X connect 102 0 89 0; +#X connect 103 0 90 0; +#X connect 104 0 91 0; +#X connect 105 0 92 0; +#X connect 106 0 93 0; +#X connect 107 0 94 0; +#X connect 108 0 95 0; +#X connect 109 0 96 0; +#X connect 110 0 97 0; +#X connect 113 0 112 0; +#X connect 114 0 112 0; +#X connect 115 0 112 0; +#X connect 116 0 112 0; +#X connect 117 0 112 0; +#X connect 118 0 112 0; +#X connect 119 0 112 0; +#X connect 120 0 112 0; +#X connect 121 0 80 0; +#X connect 122 0 80 0; +#X connect 123 0 80 0; +#X connect 124 0 80 0; +#X connect 125 0 80 0; +#X connect 126 0 64 0; +#X connect 127 0 60 0; +#X connect 128 0 16 0; +#X connect 129 0 165 0; +#X connect 130 0 160 0; +#X connect 131 0 132 0; +#X connect 132 0 128 0; +#X connect 133 0 146 0; +#X connect 134 0 133 0; +#X connect 135 0 136 0; +#X connect 136 0 165 0; +#X connect 137 0 135 0; +#X connect 139 0 141 0; +#X connect 140 0 142 0; +#X connect 143 0 147 0; +#X connect 143 0 148 0; +#X connect 144 0 151 0; +#X connect 145 0 152 0; +#X connect 146 0 127 0; +#X connect 147 0 142 1; +#X connect 148 0 141 1; +#X connect 149 0 44 0; +#X connect 150 0 149 0; +#X connect 151 0 13 0; +#X connect 152 0 14 0; +#X connect 153 0 155 0; +#X connect 154 0 153 0; +#X connect 155 0 137 0; +#X connect 156 0 20 0; +#X connect 158 0 111 0; +#X connect 159 0 44 0; +#X connect 160 0 21 0; +#X connect 161 0 44 0; +#X connect 162 0 44 0; +#X connect 163 0 45 0; +#X connect 164 0 45 0; +#X connect 165 0 160 0; +#X connect 165 0 154 0; diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino new file mode 100644 index 00000000..917f569c --- /dev/null +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino @@ -0,0 +1,351 @@ +/** + * + * Control 2 motors on ESP32 using OSC + * + * In this example, all the commands available on the serial command interface are also available via OSC. + * Also, the example is for 2 motors. If you have only 1 motor, comment out the lines for the second motor. + * + * + * + * + */ + + +#include "Arduino.h" +#include +#include +#include "ssid.h" // create this file, which defines the constants MYSSID and MYPASS +#include +#include +#include + +#include +#include +#include + + +const char ssid[] = MYSSID; // your network SSID (name) +const char pass[] = MYPASS; // your network password +WiFiUDP Udp; +IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) +const unsigned int outPort = 8000; // remote port (not needed for receive) +const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) +#define POWER_SUPPLY 7.4 + + + +MagneticSensorI2C sensor2 = MagneticSensorI2C(0x40, 14, 0xFE, 8); +MagneticSensorI2C sensor1 = MagneticSensorI2C(0x41, 14, 0xFE, 8); +BLDCMotor motor1 = BLDCMotor(7); +BLDCMotor motor2 = BLDCMotor(7); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(25, 26, 27); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(0, 4, 16); + +String m1Prefix("/M1"); +String m2Prefix("/M2"); + + +// we store seperate set-points for each motor, of course +float set_point1 = 0.0; +float set_point2 = 0.0; + + +OSCErrorCode error; +OSCBundle bundleOUT; // outgoing message, gets re-used + + + +void setupOTA(const char* hostname) { + ArduinoOTA + .setPort(8266) + .onStart([]() { + String type; + if (ArduinoOTA.getCommand() == U_FLASH) + type = "sketch"; + else // U_SPIFFS + type = "filesystem"; + + // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() + Serial.println("Start updating " + type); + }) + .onEnd([]() { + Serial.println("\nEnd"); + }) + .onProgress([](unsigned int progress, unsigned int total) { + Serial.printf("Progress: %u%%\n", (progress / (total / 100))); + }) + .onError([](ota_error_t error) { + Serial.printf("Error[%u]: ", error); + if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); + else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); + else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); + else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); + else if (error == OTA_END_ERROR) Serial.println("End Failed"); + }) + .setHostname(hostname) + .setMdnsEnabled(true); + ArduinoOTA.begin(); +} + + +void setup() { + // set pins low - motor initialization can take some time, + // and you don't want current flowing through the other motor while it happens... + pinMode(0,OUTPUT); + pinMode(4,OUTPUT); + pinMode(16,OUTPUT); + pinMode(25,OUTPUT); + pinMode(26,OUTPUT); + pinMode(27,OUTPUT); + digitalWrite(0, 0); + digitalWrite(4, 0); + digitalWrite(16, 0); + digitalWrite(25, 0); + digitalWrite(26, 0); + digitalWrite(27, 0); + + Serial.begin(115200); + delay(200); + + WiFi.begin(ssid, pass); + + Serial.print("Connecting WiFi "); + Serial.println(ssid); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); + } + Udp.begin(inPort); + Serial.println(); + Serial.print("WiFi connected. Local OSC address: "); + Serial.print(WiFi.localIP()); + Serial.print(":"); + Serial.println(inPort); + + setupOTA("smallrobot1"); + + Serial.println("Initializing motors."); + + Wire.setClock(400000); + + sensor1.init(); + motor1.linkSensor(&sensor1); + driver1.voltage_power_supply = POWER_SUPPLY; + driver1.init(); + motor1.linkDriver(&driver1); + motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; + motor1.controller = MotionControlType::velocity; + motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.I = 20; + motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.output_ramp = 1000; + motor1.LPF_velocity.Tf = 0.01; + motor1.voltage_limit = POWER_SUPPLY; + motor1.P_angle.P = 20; + motor1.init(); + motor1.initFOC(); + + sensor2.init(); + motor2.linkSensor(&sensor2); + driver2.voltage_power_supply = POWER_SUPPLY; + driver2.init(); + motor2.linkDriver(&driver2); + motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; + motor2.controller = MotionControlType::velocity; + motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.I = 20; + motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.output_ramp = 1000; + motor2.LPF_velocity.Tf = 0.01; + motor2.voltage_limit = POWER_SUPPLY; + motor2.P_angle.P = 20; + motor2.init(); + motor2.initFOC(); + + sendMotorParams(motor1, m1Prefix); + sendMotorParams(motor2, m2Prefix); + Serial.println("All initialization complete."); + _delay(1000); +} + + + + +template +void sendMessage(String& addr, T datum) { + bundleOUT.add(addr.c_str()).add(datum); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + + +float getNumber(OSCMessage &msg, int index) { + if (msg.getType(index)=='i') + return msg.getInt(index); + if (msg.getType(index)=='f') + return msg.getFloat(index); + if (msg.getType(index)=='d') + return msg.getDouble(index); + return 0; +} + + + +void motorCmd(OSCMessage &msg, int offset, BLDCMotor& motor, float* set_point, String& prefix){ + Serial.print("Command for "); + Serial.println(prefix); + if (msg.fullMatch("/P", offset)) { + motor.PID_velocity.P = getNumber(msg,0); + sendMessage(prefix+"/P", motor.PID_velocity.P); + } + else if (msg.fullMatch("/I", offset)) { + motor.PID_velocity.I = getNumber(msg,0); + sendMessage(prefix+"/I", motor.PID_velocity.I); + } + else if (msg.fullMatch("/D", offset)) { + motor.PID_velocity.D = getNumber(msg,0); + sendMessage(prefix+"/D", motor.PID_velocity.D); + } + else if (msg.fullMatch("/R", offset)) { + motor.PID_velocity.output_ramp = getNumber(msg,0); + sendMessage(prefix+"/R", motor.PID_velocity.output_ramp); + } + else if (msg.fullMatch("/F", offset)) { + motor.LPF_velocity.Tf = getNumber(msg,0); + sendMessage(prefix+"/F", motor.LPF_velocity.Tf); + } + else if (msg.fullMatch("/K", offset)) { + motor.P_angle.P = getNumber(msg,0); + sendMessage(prefix+"/K", motor.P_angle.P); + } + else if (msg.fullMatch("/N", offset)) { + motor.velocity_limit = getNumber(msg,0); + sendMessage(prefix+"/N", motor.velocity_limit); + } + else if (msg.fullMatch("/L", offset)) { + motor.voltage_limit = getNumber(msg,0); + sendMessage(prefix+"/L", motor.voltage_limit); + } + else if (msg.fullMatch("/C", offset)) { + motor.controller = (MotionControlType)msg.getInt(0); + sendMessage(prefix+"/C", motor.controller); + } + else if (msg.fullMatch("/t", offset)) { + *set_point = getNumber(msg,0); + sendMessage(prefix+"/3", *set_point); // TODO is the set-point the same as motor.target? + Serial.print("Setting set-point to "); + Serial.println(*set_point); + } + else if (msg.fullMatch("/o", offset)) { + motor.disable(); + } + else if (msg.fullMatch("/e", offset)) { + motor.enable(); + } + else if (msg.fullMatch("/params", offset)) { + sendMotorParams(motor, prefix); + } + else if (msg.fullMatch("/reinit", offset)) { + motor.disable(); + motor.init(); + motor.initFOC(); + } + +} + + + + + + +void sendMotorMonitoring() { + //Serial.println("Sending monitoring..."); + bundleOUT.add("/M1/0").add(motor1.voltage.q); + bundleOUT.add("/M1/1").add(motor1.shaft_velocity); + bundleOUT.add("/M1/2").add(motor1.shaft_angle); + bundleOUT.add("/M1/3").add(motor1.target); + bundleOUT.add("/M2/0").add(motor2.voltage.q); + bundleOUT.add("/M2/1").add(motor2.shaft_velocity); + bundleOUT.add("/M2/2").add(motor2.shaft_angle); + bundleOUT.add("/M2/3").add(motor2.target); + // TODO pack it into one message bundleOUT.add("/M2/i").add(motor2.voltage_q).add(motor2.shaft_velocity).add(motor2.shaft_angle).add(motor2.target); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + +void sendMotorParams(BLDCMotor& motor, String& prefix) { + bundleOUT.add((prefix+"/P").c_str()).add(motor.PID_velocity.P); + bundleOUT.add((prefix+"/I").c_str()).add(motor.PID_velocity.I); + bundleOUT.add((prefix+"/D").c_str()).add(motor.PID_velocity.D); + bundleOUT.add((prefix+"/R").c_str()).add(motor.PID_velocity.output_ramp); + bundleOUT.add((prefix+"/F").c_str()).add(motor.LPF_velocity.Tf); + bundleOUT.add((prefix+"/K").c_str()).add(motor.P_angle.P); + bundleOUT.add((prefix+"/N").c_str()).add(motor.velocity_limit); + bundleOUT.add((prefix+"/L").c_str()).add(motor.voltage_limit); + bundleOUT.add((prefix+"/C").c_str()).add(motor.controller); + Udp.beginPacket(outIp, outPort); + bundleOUT.send(Udp); + Udp.endPacket(); + bundleOUT.empty(); // empty the bundle to free room for a new one +} + + + + + +long lastSend = 0; +OSCMessage bundleIN; +int size; + + +void loop() { + + // FOC algorithm function + motor1.loopFOC(); + motor1.move(set_point1); + motor2.loopFOC(); + motor2.move(set_point2); + + + int size = Udp.parsePacket(); + if (size > 0) { + while (size--) { + bundleIN.fill(Udp.read()); + } + if (!bundleIN.hasError()) { + bundleIN.route("/M1", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor1, &set_point1, m1Prefix); }, 0); + bundleIN.route("/M2", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor2, &set_point2, m2Prefix); }, 0); + IPAddress ip = Udp.remoteIP(); + if (!( ip==outIp )) { + Serial.print("New connection from "); + Serial.println(ip); + outIp = ip; + sendMotorParams(motor1, m1Prefix); + sendMotorParams(motor2, m2Prefix); + } + } + else { + error = bundleIN.getError(); + Serial.print("error: "); + Serial.println(error); + } + bundleIN.empty(); + } + else { // don't receive and send in the same loop... + long now = millis(); + if (now - lastSend > 100) { + sendMotorMonitoring(); + lastSend = now; + } + } + + ArduinoOTA.handle(); +} diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/ssid.h_rename_me b/examples/osc_control_examples/osc_esp32_fullcontrol/ssid.h_rename_me new file mode 100644 index 00000000..9d3b03c6 --- /dev/null +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/ssid.h_rename_me @@ -0,0 +1,4 @@ + +#define MYSSID "yourssid" +#define MYPASS "yourpassword" + diff --git a/examples/osc_control_examples/osc_fullcontrol_screenshot.png b/examples/osc_control_examples/osc_fullcontrol_screenshot.png new file mode 100644 index 0000000000000000000000000000000000000000..214d2a81e5d105692bd0400d1aacd1498525f839 GIT binary patch literal 261643 zcma%i1z42b);8UeA|Tx&4bmMFlF}U_-7z!+(%sz>iqhSkf`oL=(A_n_5dXaAjdQ*e z|2fRX^~Bz5uN|xQUiTBBsx12q<0S?h9NaT`IcYUGIOGdBI3#W~6xf?Vl!iq(IE)o* zDJfNXDJd#d7e@Mj>sE7JwA0tV#Kj;A;8g7XZIwz)Z!kC4=`8gBqGA?a}@9mjo_)A+Xq|Y(@6w} zF$maab}G6IKQB!AVpl-hj>n4xr?T!uXAKu$h9^RsmKAzK9YSlzx{m%9tx{$_hk5lB zS7tr1Syy)t1TfTC1-#yMl2wf|Wj4P<<+=!aKJ1yeT^t$1Bg`SGcl=SjbY z%bc4tIza+o4uK{iBlZy79(_n95Tz#BYoRlgRg?GO6AP6^XtUZ$AIY|}Z{Vgaw^WcN zzvk{>D)fh`>^O(ZnMMSkZKy!vLQ_pNakEjE<;VjOTET_|zcw+cVDDsf zNxm+rW3v%WRPxR^(XG$uz*W**F8zagyJUl}eJqurxRH=0uYX54&w=_&_VJiO$XQN( zM8Z;(YujdZU&fk?5AaK;0sCl7V&$j=_Ris=(=EjpSAo4gAJ5iUgY+IzurV-19UQGX|mM_2tjtk{}*;a&0l$Eorz9(`9Tv>@LY* zT8}SoT4`$7;{wyOv*8ToCAZQ`;87MZ|F|9VmdNv_peh){%1xAL|q}L z3{ChIAvis(rzAHRzNp7R#yw`Q0@TERz4v~l@SI>P21AOzK;XRG|R! z-ZiI}{FvFKY*>TcTdR>yLQbg8U!D1!gPP(zk=xinFHXW#`l3#4&j@c2Z`5zHZgy^* z1M$1VS0&MCPtk6JM}ymg-FpB%RHoS_if^b}@CqaL`gr=@eIPg!KEpdJ4-5?K2&|P1 z4pcR*mn`lv4T-{dz{rz(P3M}(#}q+FYfp2;2#7yU0wwKW>@Y$&I9N_@Uw%tXy(&773@&ze}>*nG4~wyCobo|U%Ew3V^iwmzN|t`y)3;~fNG zdnlh})4Ebvb?a^GCFqUR?blV<#8@9#L2aYzJ*@I(D1LoD@Z2vtNIuA(B$+MVZ#lpK zi~v*)I!sEqbfOhy=r_pBG*lGW#Z|sE%2w(-8i1w6WcUwp+8JuA8_1#ZAOL=T%|laI|cskVrqmHCOYi1{~=? zXjeb`eP(|~xYsM<;@u8l6l!r~l4IRIclS9aQYn8_FUviPuVK5vV991_$^D~{SgWSH zy?f;m>*4Kn>*0xjD!C-ttdN_aFTl+unV(kxTZlxEUTB8@ip%<^)3(g^Tg&L$w2Rj} z1Y>yyWQHyVu}$}DHhYTFaUb|jS!Zsqg z-Y-4Rg&|&|mKoKPx~RI9ZDs9rMuTmlo~@phr^lxfw>Qw|P&erJn}HjxThX&Ah{A>K zCFzyNsqr=#F9014&iaaI$%v)!kYKzmG-B(lWF#ACKM zx=n6t#cwmCxQKEL6m@^<`o1AaB1`k7y2WISRv#fzF#S4R6z$U9ywuF@mD}b5;sD1O z5BTTqr|P8&CU2)U@Yw6DQkURHqqT=<_RQ_U@3l^eeGE1~98VvL9w4 z_`OY|?$V~i>oN3x2N{Sg#wx|W!FFI!(davR{zzCx=%Xdctghu=!KI_wWaCu-w13Cb z$3tRUQC@GP-*COs(gZjPSqv$~MD4hEs;HK$mQ$N<1pyw8PkwWqlnlJZ4*Q_PCa<{{hcAAmwiZ(qUzrb^Bby`YOEokka}P(2 zfbI3$j?_Z@`j3zLj%tqlQo2*@g|IyA7S4*_sRF((mbNs@lv+rQhUK*7@OgM0+bov1 zw|<1|P=<(smNq)S7SA>{6kzM8=|k)s4?z`q>0&&GQ_I>lCkMb^j~#bIcdpNrpFKmZ z!?tq>CN+=gMNPs6@o&18l>ex)R^e`M-sIqC5xQUB*~~kkKo^|!O7^PTBPukuiG3eO z*eK3=%UW0-INQ+>+>pERX;5g%a4GJ==IY>)z?<>A%J8zr_+6Vfm3GW>#+&k z)|M!OL>{8Tr2f~}&Ec*qJ3(W{ZL1z@c0{EImv0-R7}=T)1c4b|%~z2dDXFE{+Hr=i zP3~n&)va4CQ&$b7V|>?GcN~7M__WKn`$0S*p~sdW z{|72a_f79i97kLvC9_C{XTep`HD4__6G{eEzJ^{J`@OunT!Zsi-8z{BcVfK|r4=jn ztbn$e4!Q64Re?`qh%k+?I-YluQ8L|yor~ERWpvza;f#w_#16z^ccixEde)wm4=g>L z9D)I{A#oyJc~`)DsWaJ_xU0PMycW?xKcHwX1Q}9%Ub--M?I;1JQ2MB(7%qT6rokK_ ziISa((UtWe=bsG+cZCAihdlLgl`KBx7|4Y16!F#Pl*t(>zk(=W*BlPX7|}$QaB`C7 zk!h%vDm!r35Z=y`k@5-O`g_h%9eX0`WTtR#x~DA$T*dQ%%3=b74FCaw@j?7YN`#37 zv6VTMNbK{0A$7#`J4_r&oz)Yrm5Zlu1D)e0T!=oWklQ>l0I4r`Nv1I@-u+;%BX6Ok z1os+tj|PVTj}M0kyMu@Q;KHookp6ZL2gd-r!oeXXhQgu3uK2Kj)qI3MOOY?~5&yhL z;{L6ogu0ZxJnX7&=3;K{;A-XQM)0Bf9UL5jn6-wEn~svApqZmRo9Rc#59Vy1_D;W% zzzKT_!fx%&-At)G?d=>~1wBP*{#HW}cK_RJb{eX`RdKTwq0v!NrIK=VF{k2Td&9;- zBZ@&qMJ4R=(Lzv7TILUO*pmp2m7ANBAUnHY3^zLcTNtj ze+&yYK=$8C*g4ra*#8}wo3+J%Ap5Q4ce1~Y>vwj-zcnVP>1ysGjzM`1gMPPN-__X>O+@Z4IMzg^fv+lZT7@53+wQ`nROI|3J#Y$McV*|4{S~q`wV9 zP{qX>)>G5pW>J(=nEhYg{ZU_-{kJLphbjMkJ^%I=wxFUI!t8$uhbYG3MAuI^IB__6 zX$cKa_`@93^ymGvp*U3mn{}_KbR`OCh<|KH=OfIKbw|U~qI8Mh5Wd%F8|I0E=LjU` zh*t`Dhm8>#`4NfkYbQw0W|!RC+rr(_-O@uvL}=;T`qGTZlyQ!zrkwP3x=II{{Zd0( zvH#bZA`VM2&hK8ZFGl%e5~d0J=_+{iarI z2PH#om3ZKO{JBlRWY`k@oLS3&_W@dIx75H|Xur*W6`&$B`eSLv_ast;;r!%_f3e%Q zNnTr0u;wUPqxH`QOh*hi|jqxyC6dsAOg8#?x(p0g4^FO z3>ta3fxYMue0qaMf*wc)4WTPsh1o+7Fq`amJ2>t z)eCjj>P(rbsm|bS87p^(g_^h9qgiQtkJ_bLO0JztuS)02c%Y*x4uzRR$0#4P7Kn_1 zbx`94E^|o;&c2-A?Rd)QL)r;b*ks)WeUa?^UzkB;p>q!_Oys3hJ_nHA4uxur5lNuY2fXO zf%IaX11qK98$D4{NV9?0Euq@y56E8Y5#84y&&6+Fhkr2Uyz&6~#bqz8kqK;EPHk0o zrv8Ygv>d_7Jn^ayN{+uB>1 zr`Xwfomi(T^+aaw{xSXw>ajwav&y=T%(z?i@}u7#SIy65f5;XA1v@S8)e2Faug|r`BR?YW1ZNSN5)|)7iCAyT#O_at)3eL!!{nNqf(W;aB{$ ze2!$-!Go?|=6z#BW96;B24tP)k!f+*yA_m6O%vQ-t+UhmLA;a>z!O4tqh*Kg>^Iqs zl)xKM1*Q3=z#HSQ4~dXjdwuimpBX-`9GHVKBB8pTw@(g>$#_4$`X(lGwlH;@o2j%e zpsn#Sf3`SW$;vg-)v7cyS38O^U)Qeq*)h}5QTN$tjYh59MpL)irFb;YpG~DTNRC{{ zJ$Y&rXO%#iUt)i@+!k`tOa3{1qt*SWW<)itj#E))dJ<{-RfVV&KPU>yJYI$xF%06-?XpJt~lDvfHnLJJ4+rDTkl3rI-eY{q&t$n zfq~3XFCJ)Y=PI}t!!*UOBW|-xC`7!6x;`P66TV5EKstGA@3AXJpO2IrfUk=1P8|Ie z{J(#o`iwNOPGZIUz95(<`sw6ie8jf0liJ$)Z8@hI^@Yt|ZcN*lqv8#Y$ffE+3!t<< zSH+6;4fnrq>;G_}vOs8(?QER$V0=^+e!m^z!|G?9|^I zl0$%xK8+K&v&sT&^3!S=Xmr&x-v* zQeCv}(I6=^-S{;8}Asd8(GF*+2b`A_3m3(c!fMay?_ zMQRbFYNG9B$;`%T!$USqZSIkFMnc9IX=N0(`C@(>EF@K%BwmgqmgkN?%GDU!vy)#L zztei5$DG6qWWShWO?*lJ{ab$CRJ-zs`rl;elknTs&Zmb;4VaZ4PG(B_x_|UKYv@iT zokIfnaHaiCzp-LOlzPPf@m3b(x$(?;BLCCREWYs$>K{vqY=`sJQZPj|^S3Q7E^b~C zsBr_>g4(=dlhalt+pa>k!2VCH%vxo*KyWH=r@CvVvHMa|*`;IWL&HP6afLy1(Q{6R zW&YcP%FZIKay|JUVvXEN(|+{2*#$YqM&rQi1Jj$qBZUG@qvgPa_u9-{RBTU;s`D^I>u#}Ru6$6Kyu$|?7kII z&yP?*WdkL*WySZ@inrig?V2OhZ!!-GFz*Iv{O2mAir=j@t6g@z{s1CGOEGMcW@3^8 zl5P*X5PX#@3@gC6)7ccOy)@6zv`Zk=C$@M*#ho%;dgx*Ha!{z{@e>QqF9#LGXU$B&2*5Y#j9cHeaztAkx;aq6*swHg3 zO{;9bUKAGM-x@q>-pNf;R~C4S4YLWIz0f16n=C=VcemXM>NURw!s|gwf4K;J`oz>u zX2!-(LT0VxUxl(uCx^;8g2-B1VU)o5Ib(mr{poLJCvmXl z>M-zsxKir`K6JVL+9ycY!Fm0cxkINmLH4ujjGoO21(O5k&(M9XxGMm|>_)^4nTQ_$~m9c$_qaRkgTW zW3F)OnD{Ei1%PvJNI;KsVw#rcVeCF<33JjhqgRV5xbEcI6(Zq<7!r)`&s&?MNX~$HU0(f;TNSW$p=O~pzF?9{fi4! zGAHq$)&*N6z~w8=Igj4BovS11diM9Xo?ZhkKre-27(-s&g7Npl_IF!p#+z*yV@b5J z{4~~nzeX6?D^xN*7%XN=5TUq|q<@_7=n8`zKY+*`8jMW%;TRdH=&HmvfwsfhA$p?{=zdR9LFkI)e ziROfssVZLD(+~%odDcAt0a@UHt*4KA<_#3#fFw}Vc{ZcTWnck{4J|w36g^iUIqQaLF*x@tfKV4Kb!3EgW|Fp zAm86?#05eTmaE*37BydMl;8?dbOfr7BJv@ad3(3i*)2?pCX2xw5%24QBG+!!S_>rs z$g*oWT-Zp8EQOb}vhbO7#vALz`Vyi2lETfy+VKXQJG$7^)S8alZPIT>?JWkbqdb8( z7guw}OD!e~9*fpr(Y;!5k38U0(_tFhY+~*`A%Uu^a0gr3#Z3uCXI4a7gETXJY+?ZYZnou`>V&M`h8-{%q6>ujh3S7TX*c4MR- z=;U6XfWa-Sg7>Gr^7!-qG*|W&7x@jY>^Ij-jz{$|$Se-u*;;mKTI0;ZzwKnZ#vXM} zQ!nxyc(j{EKu>#dmVCNS*LuzGZ_Zq%l=#r+B#T`4hYQ5~&wnu1xNN;^zMNE8u&)$A z8Lw!0-D?#JC3t8z8;qUx7MyJVbz(4KY?!xI9{%)j;;+{1V%~h(gGoCZ=k03z-LSO* z&>HCU<7%{h9C=?+_|6tKPb})^^wNUbWSA{%{KrIr72NBs*VW3Mac(#mud{?bNq&5&ai+S-8omz)cUaB6eqBGYU=pCo@sNL6kZF3U-Wyx`K7u$ zA8lAf9ps!yj`4v%E!PsU8NAZ`u2Ef3)w-j8;>62s@w3X2lE-d-0@N6B0W+W&j(8K} zxcSX-P1kpRs7VaFWUkBXNI6;gw)CauA&n`(pY6}jvdyY??oUNR<|)d-7wu?#kQFKa z9pC_j-0Li7#DfSrzYRg%DmQGMrMNKb;T8j*4N~}&DM^tXLz~X2aagXK<63>XVHTYQ z7LH^sJM})h_7N-*l{mVEN%V)7oov9Y?lqKQR&;MpX65Uj9!ZQ~)dzgP*q|iM)%b}L z?j9}bM)yAIITub4$sQMe)({lQ6FXgO{gBg{cDuF_OIJ)EBZ>0U;m`-yluxESRbG$icf@|V+z zpS@?_zUJCIpj@0P0mJFo+bZ@Z2=g^3Qk&S&12>PYWd;PIY2`AHg@$YYxF2Y?(q})YnZ|zp_2+IeBx?+Ucogl<762S+byvl zKXZ`7>$*bq!Uf0F3G);CZng_Wh5{UCtOPeB=$FSXqn*0Peu)2Z5RT})k)s4;;1u|_ z-5{UcV5NSASS}Wpg01+%qh+`!!{H8Iw*4*6%5iruFZQ?>f&oNBX*<2Xkn8( zfke3FgbY60m-wGX9KxNXXd793_4+;DM_V$C@jbg&OMiYFng%8E z>JGGx_Y1bVLbp*VgJgR`i;PJDc~8ySY{PG%0oU+Hh__#Hh-X%SeENJsAOc&+-2=Dr zc{R#8cPB<*p{&rcJh=jJd)iYCsSvLL{)N2Hcz{Xn1O`+g0Hx`mC9$SkI>HsP^>KiY{Z?&kSVn#`Wi=+}M@;m5g%sM%V zlr%3T(Q)>W%8K!%ansK%708gRklaN31|xA5T6HErSVyTIBfQ8&S`1adr&wNF^L0|E zpzI823RoTNg8PK($AW0>@#f2BgilCeoao}0eheIHOgzklPmQ!yNah^9$SCW0z z`C)(iNa$Dz;hd^Nzs0*I+6`Fi`s|JM8{NSABNi8oNcPZD0yzpZj^fHNr~i z`<>EjGvQi_qArP#*^@T>C?AST<2nI=Cm4(oR5kV(KWiN`Ak{fX-YmT|Lk&U=$6(APcE1$Uv_KusL$BNaKb)N`fLvNxmFglh4H-|Ku*zvko zTAwLLs02T7u<9Aqa@JvZp&ZMv3LNO z=;41yI!El5S#brZyuY@m7 z%?vd@2PZur8;feaGQ8)-I}W)`8-J*TlhSv7DJC6SFCPj1;b5;mkSSTU)pyA*=DI&lwT9jyjyli}ra_8;SIqL|zR#na5VGlJN3w^Fv?ZN;YE} zs=ZWvFm+&&!bxlrPPhFAO;33e4t%M6r@QI1z4KZ;+IuPC-u*?=ijF%QeaaKjS-dV> zrIMghR4iP0Lnq;R2IZoT@ZEQwht-3JlMlTw6Wyj)e2(!Dyjp`K=%n9!uDPRCXRm7W zIeKdwr#h_UkZKzh{ow%06+Hdy5r2v~t}Ki4uMGq@A_m^Y8B;p>k$%ov9R)ABU$@#u zJfPJY^!oA-03=qIEq1&aaU7egv(@{g_r)oDSqTwU44`!}mpYpt>Z9dG>VxZj>kjzu z4OAJIyKsr+T*hBWToo!W&xQ92e)7R&)0rP2d;xjAFZ(zR-Xe9Yz;#1UsK5-iL3v|f zBA&!YPGe}9tZ+Ncq(`~#OF)yNB)SGsX|PtQJIFY1he!=OBRa(z7xChztuA&8KFkys z#$cT`+mDGI_}=gTY)=ww-C!XnWtsI#8bvJ{6t_dJ%G=QBwOCWb!5{<@vrUK67lJ6C)wH_=Ft7&6qf-Qr1t~rM>ck?YS?9)m#T)KCXSsNnOg@3IViR z3ra7C#WwkH(rGOM{h;49>^U5Q4$6m&*qN*xkN&!hFr7X*3UNGUr!)yNBm#;|yj5r2 zKzhb?IvNsqyde8N(b%3&0X|MAVoeVXbLJ6Cam2BmTULsOzS^;kaiSWQjB zxb(N4E3-{?EQU^A&bdwm5+*qh9GgMKB_ng>9zkrp62bmx`3}d3j?W#94rQ~8aJq8L z*c^iiubaDg{U#!e6Ygl*_8f6UZ}JIE4ZTxAHSGtN#-ic2Kgk2;`?6kQ$$1{gygUgL zK8lm8rEDafj_DHY$!o-FJXXs4EzFViauCTb7TO;Em=)j@6A*XhT=z&Y-B?(^l5((5 zR7ZZobC5=akjczEDe#tV2NaUh8WrV(&C*@1yj_ktMy}PeY?*x!>se{WpCslOa%zEc zVSLIEK#CTjAvS5!{8I$&eL`3LQH8mI%rC~1mIJmWaFwJ53ybM<`0Q_PW2RBYq7+Cw+^$Ua&aVedJ0ZCT z4b5>aH;1lvRoCv08P#B9$J{d6151e=;$M8iP3P@^XHUTJX`)&^t)d1sJ`{TrgEc5V zpNy}kSx5uwmCv03L;=YgA;_*NZ=KfNx@813*Hv!LC0gH|FCJ8TRJMCiik4g!&9|uv)w(7L48k0aY z`$>%pgPiNQe0spIskY3c9(;*c(~gebS?pdXqj_e1)DCs`m!3}ks-nZPu)yfk{RC>& z!~ho=Wv_78c~=Y%HNx{Fvl`Wg>Z<<93{(Ro_E>5?&T7&MfLv7ZQWSd!@fj3(M4^vW zn%ih+M}mYpMI0!6xSkjkX2J3?V(P03rLD-IGX!_U`^LS^dIe6j2NjVd+>FgQtGIW@A=AQPIcM=J#N){BVo`VgR}#ehFlkQAw{yb)!J55Bkg&iY!D9Mrr}0> z%kkDzvPit4GT$8LXDNG<52$c_RgIhi{fQK8}z5q{X&!X)saVJ)<#1 z@Inh`XVy@ijbd+iX*rK1z6iKHlIyu1q59;y6W8?XPIp3pp&iV)yNq-089k0gbmt#KIxo+q%9vS?t zN(NazML*@#f8O+z1M92b`M#gjezZGhCeOqrniJI7tmKt^bRe+gb4J{k;na-ON_9Xr zr0dmcW8!WYO@W!C*Z2K&O)qof>uW>)Htpd}9NbY<{kBHCG;@OgMnRf6_f zwbqsof^lbf5gI|?+ai}2PB)cX-5C+a)lIU=CNvU(U=OUl3^W{P5>Aql_rAR?KisQ_ zzZg*Dd*#b;Z1H#?*&x9Y8Gu0MZPs|KH>ZoDo$z`McGe@bDvlivD=9~*Xc&eqZqqEl*O+XXRcb&DaLq8{|5W7#R;2R60H0aNNlFn0gePJsnOW z|E3vAcliyi=Y|4ywZ|(Kl++;G*{!wI@GV$=@iXC7m0p~ia>gotH^mD!);Yu=cmuvm z%IhT1C`mUjbfZ0X1@O*$IfCjrS64*ksoWK91uXsDD3$((sPkqlj&COxUiFUGFXbRX z0(BBOhnX1FkcbR|nNYz!Ok1@{FdY_7q`{k=&K(S|f1HPfpOELn&G?GdOK7Z&JN^X_ zSXlqbtwIC@IKOgOdUi2hpK&6CqVB{P$}ze*uqMe^Zf9BUnLw8-;( zJj<~Twr`C!Y-ST{#1iC_fOP&%T|kQB!8_$l2xZG6IV~`Mu+3nins{qq($~Bu6n&6e zhxO%@7ZXv!*Pv-LQQ=Tu+hM77(*tP_?YE;4NIU?Y$7NH_b~#{6Z0k&#2@3 z$y#8dWG8FGFUWKSbJJS4x4%mNyI(a!Zfz3{TWTLi`I+k6BcFr#=O~s8za#*9gWf*o zu4{7j$2FWmu2ar$?;^OYdp)=;T?dK)F$%Md6@aRtN)Vp_Vh zRWb_VDj5TaP$+S%Ci4dpc(F^JrV!NoJu{^+lina!_%#ZNuiBMH83pROuF#}CaV*la zB0vz*SbGSc56Y{m9xNQR8V4@TSmYF`ji z1XvwTAl&L+L~AF3jhz8%VF%%*`>`HEmGNAWBNJs$y~V)#H^+plheUyQG&|9vWK89J zVvFIc+jqKkEnZVK6uF}JO<-Pg4!HPmTPo1q9nA4QMNBu=PP%uVuXM<5HEkrMPUp2W zT^wn%_wfTaxYHA6;&Tq>^}iTDG#PV!V}OVce`W>FiTI`1&YVyvl1N}}G_x7_1yyhp zg_+lhJ}5awP%-auA}S%G!H zZIQt?-To!4wNHYAa~={q1C{LF7w4({Ug6h<#M4?G9jWhARhvhZys`Grd5}4A#E#1UKv3 zVKQ|AA&=MU%0z1$L?4EFv#>B9Ry5i1U{C8maY~^hljDt)PlpAKzu0q*|Hwk#D|Mmw z0Kp;3(^o|!XDDpf_38OT>xwk*<(McAQi2~9Pesp;}; zB#YOq|J|w3*kgXjZeM|%wfxC5kSkHS1IW!`B@1+fsn;X?eokC#pVX+)>VdJuPhzlV zB%&c_rmaMb^5hnK{D5ym@F!GDvaaQZe0&T*mTTpkl>}u7izhemK!r&wk zQ-3t&iQ@`fPDmXw3KUyctRdmPJpEy~)P(LLsq0C$qRa;0j*>XSSGg6d0`T3M`=-uV zso5?*Lj>Mi=3db^NB({$Z)FEOfZUdg$n$Sy7XNt`U`No!^;9!b8NV(gTlL{_vRI)U ztO#lGYfzZL`fn%(7+~O&9p0*a=dAsrMN_d`CGEPyAXLw=;oU^vU)kILoP!r{K-Xax z8sIT{i69y%D$##$=Q{I0@T3b4m^zN}WVhZNDiXOl0uX@dik^>3|0VB#+Qh%l;GZar zp%?H%hV8d$77Se}TO(+h3Ju>KOh9HgHIh&zWdyEyn6lZbKmH z)=sxp|9QgyUZNHTw8%{B0UVVLdE>9@qo6dfN2CQp6&zewaX zRo7-C+jA{y@aKs?#r&_l^uLVo(=<&$(!He2bYtr6*om@@=6l=1zmleZ3eMlh@p}jI zSMj@yMU?@+0mQ6HNJIK;D~z3fS&jU6z)2Abyc?83DKmZe`7rJU-X+g}V)1t{jw-@8 zDY!~OSdy8CkPGEj(RT!!kJ({gL*4dIFv369{l5c*byU355~MHr@(H$VJ(5BAL(T}U zFUAty7s@WUkj^;L<&5uTZvZ9`{!7=t$#vNVJNj)G9@Y}17)ybut87MJlZXM@%1@U= zT+O#KQ+hgvQ(ljJO2NkB34XAveF|lEKsy>ACEORdO!?e^(e88Ia77{DT@?Lf(N-tc z;pd>GG@|wY!$^sv7a_qjZ=d`qd6%hv4M12<=I8sAavBMt9gMD97w++3nLRA($LKi8 z?{MiBAd=g-i$NTDw=KcANxR;+j_D{WF(a}iOsNuj>|Gb)X!V;g85ZOIk?Hlf;F4o z|NbGY%BVt7;M@E9&^m`--eO3sdUf-T#zo|?Ot(srl)L}V*bj)NiVx`JMXG_v8X#kz z%Vl#&acPYd5d{kkT)gzB2o&IV`H?#8xnqGYTD2KZ1o>hkjiCM zLbFItFCiM;2|f`k?=~j$&HGtST%C6Jdc`}3f}h53jI}Ba>C5BnY3=nJl2EQ*$|#C> zTpxxH(vXMKMbcg}LJfWm;DB%BDBird-6Ndh*8UZKD*+fG({214AFm|1Kzi4Z8}t&d z+PB{fZI_3sCCxZvG5IP@au#{Ufk)X?C!l>y8zE=Yw@x3#9MAg&;R093P$7Poj?%ZD zNPc|A%AVxklJuaKo$OZF|2&3Gb_8p|J^SNqRmN47M~B36I0WEVZb4*>n}u@gHwX8J z+muWq_I^)E5Ig8K!8wFsxV+`<_u6A^1muu_S_2uASO9kH{qO*KzK)B~`zc*?OYHzg zv0&x07(5YY?e&I@HF@%xKmkEcV(QI7+Sjtv2mU2f+ zEIgSQ3PJ@9eTq4_9VUYzMnsR%{^Sf9wSQ5NO)~`IwMxC_o`$N4z6gTytegtNR))GC zO$+i3^&XPU*@zhe@L=0d;O+SasQ&jsRU(#)4d>dVtewKAc%UvtNqA!pXa3YH*i;ZB zgy)0ko) z#9G>b#4EPDPc>PsFQxgjnw z5(oElp+23ZFikdOTX}O^6Np*{Kb+^j??!j6UGy{aGvg_vuXA%JI8k1XRIg0cUTqE z;?*JM@`)4P#hpSWYKzmz2ny;Le^>j;++Os>$Eoj19Y@7a%TE&zpKX_xzxe69w4~(XVTtpA|3luMp_VMecl&1lr`}2h!aLRKH-h2+V`C z!AbY;>tD~nt*ozCN*;W?BAI5~-0#<1v3;g5GaT-b#qZVsa<-mvl507z*&(EbruFR? zl?(?lww%0l;xRhkrMLu-Nk=}WwzOs)@kZVTAZ<;0aVR8!wS=)!W`wh zlezS%%e!KC%X&5=JW7Z!Fegv!Awbwsj8M+WW zOp5RLB+6EE%0`r;2H&+SJB%AZVRqQq``q(QX#(sTX%F~^E6zLQ9&#`nT0G^o^ENV3 z4H`t;6nV}o8RY%!Lnb1X?#}Q`aX!1Vc>twZ>a7SrA6*^6!Ia#3N}7~bS$X^3#jv2A zrx0~w_zO0gR(I;Me!H9l{Nf3&`wp^?-HP==!F`^@?<-DK4Ao+1oqvB0I9wJ z8KdJ?(8$nI9NyWjA0CA_Fi6PS>I{c3+^Pm zIN%sk?=4d>#7qn??>cnw;u@l{iWe21hjI1Oh8%{1XTCa^(~i0?Lzs5^XWNT}-}M@7 zePbPd10w&#RO{^2Cv-Ao{WXWd*#DuPMXydy(0!loYJUdvXM*=A2f>~)4DR|am4(R? z7DY~%>2fDqz_3^6M||L+?tym{zgSsWm9)xqs5b^;lDu4Z-k~RL$MAuw)}hVhLW^#N z(V8DGUa)Q^RhtzX+@AOJb-s;y@oek%ES;0fa*WJ+x+EdnZ%p9xgVJO6?WVAWNX*S;!`vQE zgKZGr5vh+$w>OaY@r0}JBRcwdjcyz@f>TpUwT4b?fk^~$fMOfK^3&7GwVF}95c{4Lv9z`%AyB6;y=L^thc>MN3OG6A)&zXG3} zcHH~w$6GfaGt!`ejjYF@pH#N-$hCYi9xJ+dK9Fh(oI)xhGnfDy=+k!xl2oVTPLJCM z3!k&!Kfa(__c;klSl_a!O=Hu4%}q3hMW*<3^p5`fq4a}{f`;E4%75NHoa4YRZxpUw zR1>V$Bpx~`#8FJPD3J;|IrZVm>TtG$SR0XEMBzU5z;Nv5X@(3ifP9KG4AQS{>>8uQ zo$v4O1ubvfTvuRAXaxJJcW?|E`QDlz2wCMsv;q#aK7TH2^@5l4TqO5D1q7m_4bldbD0 zsx;2#a8$bn^abU}B5~zH4a3^{U{wdIjjg3Rw3Q- zUvm?e96Cj&On^mo#Mm5E?Oqoa0w?zu#W~K6M9+yiV3_LhLlcn!1YaTw!1ViTKJ>kz zu#L?TkG*dL6Q4D{squ~wJwgH%v#x>;lgV8)cWgKJF)Pt^jmwPM^*T5aA6#QhC4Ri4 z-)ihkoqy95+fs9&dA`S(HxK5QCHJc{@IC9l*3o)=fHd!Hwj5NBzngrRbTW+1;m`^d z5w+_y`>~ur4)J?{;>7!$v@@z-nTP@Q`BMF$*GGqrwTgIdQvc-D;pyY05ri$*KMY3R zrtb=IKhULG*E-CWWh{FK+G(bAqT$%S6#j>tL^Et8 z;hs-HIGcAul}T*mEyIoK9Z68(;}{y}m;pw0>Xrn0gpeqrR3Gna45j$=VnsE&1noZA zE=SeMH6Tw^|NcT2n(cu`V+Uk*u9f2B${|ct9?#luH?IJ9Z@=p>w6=Vl5!7Q%ubm}5 zONtY_(J4O%dp-be_rB(Rz(1FbC8mZf`!z0uYQHA)CS|hew(-M0v3Sh$FH*@9by*7` zTKx1>TG1rw&~j_heBE+jhj_wuvB?63I@*?WtqOvF#9{3+CUs=JQuohgxw55T*fXEn z;x#*UD!vtQ?v3GFbc+)aRZYEbmVT$fLaoWGo#5lR!gqUgYcWQSf<3l3A(88KXMJn` zBe4nS_#RFM@jmu;pzX}F`P;?F0RP$@sRP<)WXXkZ zSVyR#+HoV|u`4H9^4Ty3L|%H|Agcpl-LA(^fr@ctiLZy69}`>lP`At0rs zbg6WABOMCT-6ag&-3Ul`iIhn9AVYUINXO9KHN<L%V_f#ZX#z4BR#++tan9RQtQi5;NFlc$2?x|zodsIK1e*5JN z_M;MaE7xzE_+WdC;p+*D-mUM&QDdGdM7V1)B;ro%Q#4l?q9F;PH_^+_B^n>>n_(J^ zUKb6kj4CIkny)z{#xm{nSq^PNpR!R8lQ39YWu+kk62GP^NPahRda$j4+4RmAL9-Eg zVRzY1)x@&kMTl_(tV647qVEk&BMD+e?ub|)v&Cs%=H1BFV+WC~NVl_uNLwa#Q^nU5 ziicvATwJGmk6CTY;~kr`Oi*j`{tWiY*v!UK(?zs*FW&5o$=;-SbYO4O#Mi_?#Ag+@ zinqEAWq=z;Svr=S4DSMVb7DF5ydz2|Jt9N%AEoyH(M1x(;e>-NA#Yw^0Bk0SrmiyI zI#Rl62<)9&QNJl7>Yz`p$YBp*Irc-scJk$V?}&1SXHPrwbq^juqFCmieZL*MztZTJ z`Ljdizn80EzEm(~zS7xLV|&BWRK94Z+N`nsbDn@ZMJMv5mOmYh)W-hcITZTsj=Aw< zfu3$CDd}agg%A@Ix{BPF+(Ce1;^A%5_l-=o#N-3vhg3Q0fO?yz$*qJW@AFz$YCo@@ zK?{y8UxJ?lto)#)wVmAgCM&s+IP6e}O~*s`cX1BeZAC-Ig$fJ23o|V?&2qIFObeb< zUsGD0Sf|cn9X1)7`sXtb;Dqo$5oSbdWU~kT2yKhG5<(n9O?pd(Epi&*%XyTfrV?|{ zV+w|Abp@og`Z;E@;0rAuf4+Ok^qWPYyN81@q_brh4{DGoi13>iDCv#g;W|~`YLEEv zO&DP1QTs~a5Rk-9%@x=b`jno(NAhR!21JQ#ZGkgM56rvW;`A>ditmF=~~r5gH8{N~kbcGK^lp+0zMh@yi4 zqkN8ONcL&Uon552%)ix#z|T|xVI$5NuP;4!H@ikpQOB1m*ff@fk{WnZl7&9ES(4YG zVa3>HO-h5Gmu?4kxZOHIz0@(N_Jkz7)+UeV&2}eF(PzHilLHK-&&3uA^YY0BW_t=tbR2&AAo0!wFAv|`P1X5K)$g0 zXqHr_1yPkIa!1-B_3s4JIly)Zj zC|q)THv8JD5Ulf0WI6N`fTKE>xNRNK)|6#0&VL}+I?GIsX^G>rJdF8sRLiCyU!;?b zC3vXhXHf86r|XN;EFlzmEgez<{X} z+)Iq+7d^15?MXUk&*C#i?+{O{Mv;o?`YUA&;ifQJF7$ zH6M9QAGbTxEpx187+>;YHvu4i%A#FrL{Ne0vmBX9jRxoqxUOsKeEoo8AZBjK$dJ2j z=yt$0Z|kgYs9HK{8~((H)05@Ec?7AxIz-mBX#k8)oeSWd66C(O#?9Z_(+_bXOBv^e zcF1E-yhTw|TG;-G)ZOqp6D@%@czEnx-V>F=HgXzY9D@&7_N>X{_){UBCtLFA@wedn zT@G8Ms>urN`XbPV2U7*?=76DVn0CT$7-^Ojcwpy0aT|a!h74!mm96xH$)q&sbZF}9 zZEyBViI0HdH9+B`+MZHo= zi&J5ruQuvM<(qApkhr~3rp4Hf&zI?sI(c9<>lhBnMH&RmKcZ&SKR<0O1tTXDn{~WN zVtLR4BVS;1wl(_l2pQDbRmM>;+mAakrGdB;n>jsxuSE1|u{8%EUtsYrnV}w58y(Lc z>3By>_*iy8%!f3ZTm2qIS+v*iFt&jjRTHqz;Ru4~8bP`3w*vwZ>LPyUXQ>nlwaCJKeB|I6t8gS- ztv)q--qUBe5NUB26D%Iuj5=$hr)J-L{*;GY6YB@ zy*-L@2hdek`vPC#=vY4#R-ym~;l=eitAaXUP&oFhwB!_0-RTcl*ewEnWD7mrQqF@N z^J6yjm;rC+h|}P^;sIjIEc3B}%>lK}+6Y@%E6gihJm_gV3zP(n1<|=L*Prkj=@DC- z{j-Tn5(`5A?hjTWNzr;H@-Ka6Cx|{1s_W4$ZqqD_;EjZsGC^?18bPZpsAMi5i!Oz} zYy_dMr_cLz`3`7u5Zus28!nuFY<2SFZ-MTqmW!)fdwyoLUZ}FBZ$$IY&|Eyrj-`}s zWl`kD@BHv0%I}fe1x=yi%A103c~sWbvllHs9^pqI?}HINOzAV)hCDH1v;vJ!?>U=c z=S71gA7ThPA)H8gsCE=>cz@V{*Q=Hfy z?jVZUpNbCA>Q8y$(mPC2*dG&k!|A z-Htxu-p000VPZCo4@E@kkN9G^f1ajGh_xXUUBF{!X1%v`t8m3Lk3ZqtLdLM5e@9`t zGA{5CL1UIh{3?bZ!MMX`W1e9E6FLX0k|dR4^lO9KbU-K)z;4}k$EGcM&oo^yyH|Rxwt*J!fX)Y6d2?XnxYph`eDC9V)?*xm z{h(SXPvXwkHF8qt>x@vv+6+%9>w|LzbiquT2A-kX$D;FIZbK87{}gN90%Lxoconf` z)@Y1O(^z4i5{Xu3=BQ_!zOGOokjCvgHsN_opSC-8x>om$wN@WF`NO!{^~@E@6^Cd6 z)f%*ImT@zDCzYZh-|>Ud&5OHryWZS8K*}vYZLiL=Ffxh5mDdt6DI;novXuh8eXG}E zH#%~qZmaR3P9vsogJj|>d_7H!EiD2IA&>nZO0dnKjf3axPyMN{q7;62fnRtD%6JXD z5??o`X~I8GW21sQjRImDF|vkoPXlEuL*2`+hNEUj2Jp_+mV@TAr`%(nFA94(o;@zM zr;&HewGNt1<5ME*jO@n?0w3-dD>=TOw1ZnE0p)&jKGIiI!Vd;rl!1(e?r@D@MqCic z$Xr!-b%ZMyA>!D{1$0Lt{;#a0>TXdGbS3#l_@xv_5fJHjCJ|u(;|720q-hkrRbOWg)!S&cH?S;#`#2u58kO8 zUS8eV$g}7Liu%rI_E9FXn`Odt)2deFtMd|$RZh(5bHS^cAr^K}wpm^Er~EMKVOBeR zW{yvQv-3$ibA`DRtgEqhhs|1n1`k$|Jw=OP!-9smV~ z56P`6C1DRsx_I@tFygfWQk3UbvgRhF))e3OoHS8uAvl0!sOx|Y`Z82bPM_~CNo|wS zR4E37cAMjEWvc~QhMx8YYukizjL*bqrHt+ozIn+rJSAU9z?3w1+r#1$ka=t2>==!;XXVhbK*3nJ}$ur}D~QuB4`Lv(&OJftzu*-xT{Pw~XyZX@%D7G*9Gw6GKI|8n0(F z_Re4S#$4K^r`0!8=dP*dVdGg6#ha2Hm)8thk>Z%b@-@bS^Psf zJA$V1#W^I@Er3LL*cW}LMeen0Ow4=CIV57!OCtVn#Rd++>K4MB6ZV0TH1Z{lqazf$ z-J3ks3^HuYs4B!iW$@=6*dAa!Lc3jY=y|Q_!HY5MRklMzAo3V~SzGauh}JRB@A2LO z;rG40T56LbTwdb^zXe{;Du?cPpRJ>Q=WTSa4SAy)cBUuUsa&kOk6bLQ&?Dn3DGJIT z0MWMYE*IxmLQ`H5vK+~>MlM0eXF#!lohz`0mYuiYcKLqxj{Tt9C(8NMy~;CNMJC`9 zXIpt>&urE5l)l`8mpgxJDYBK6m~T#c#`l!2!k5>AJ91L>lrHGPC-Zdjj3OO|1(={~~lMmLzpwthUeZmLbCJ zAq%$$*tr3(DXx7UIyrmE77TlpjI23v#GXe>+D`(_(C()FyCd8Y^qDt;82V>6`s1sy zuRpHft6-WK?=TVx-J&-iKP@{cV6W%@Ki&sYq;Zj~Fc9-am;PtkU#f`11wZ}Pb6W7{ z@5!RvDX>Q_{L)hg5d1&pcAQzLQEo77__SxnSz$mB91)zV;%|p6Rr?*-MGc}3Y@{K5 zM_7qs9Fa5?Z>prs{e{+7%b&x1q($f5BR=y6Bl>yzoj$>8+QH>NjmwVF!Iyh(BitHG zosC@UH>>`<68C1FTlI5?4X~Z7Prk8d#T1}^#@P`-FerV;S=0}0p@?maWbaX-!`OZT zK2h>sf8N>YtX02lLFCeYjkT|1W?I^BS(}bSSu+rEWu*^_uOA0uDB7kr zmY+oH&=BK>x9^H~tRQYKzj#oq=$&vFti22gMcSEzR;}Ot2#sRg0SI0yEz|RAGpFJF zi3c)rh=Z41fsFqf2a@H2ze&0KMkZ*sI&2)t(gTbrVTA{&@*_^%oXKqNZa%GwEs+c= zxlE)y_Ho)6mU1ID+=bFJBu?%(&61yL65Zb>`hYh^!wsiQj2rx*UY@qYx=zcXb8<{) zkAoAw{E~1~qC#dp8zfYP$&Qu5C))=gu=tAS4jKP37|NlYxWOr~w6TyJ)1Ry%qLlJ{ z(RkUivmW=_xj?9LZ&v9R>=0e;b!5IIIE4#>Vo>p=B+o_LK#r2PbTy*KJ0( ziF{xY;`0D9^tcYywlNs5v}eA zU>T;@$z%Ky-DBV4DE5TBnU882H)P(^sNXQuite~&HoG>{AHN5}P1FH8$Ye+AqqwNO zPsTr)mcX}2S;c7EuQ4L;4iyVRTjyT*>@rH0KOcJ-M7i4;QLE^MmFcv|dqgnAjo~WA z#-VzN7&TrQx*)7v;0ibRN4Eyw50)>3>LtGVV=iaDKw7#YFi0)a(G#_P;L5e6kMS7* zH*~1_O*fxd3@(gWa^c$lhM_=dyh0T-*+%Z8{SHDKC7!bLx6(`C#!zPGBkso1B2!x6 zDQ3FDu^TlOX`~OG%L8I<>|LruKHrQz9K&eLheN74z+;r@!rt~wN?!$)?RD8?epPAq zyE{4!1rhVw1JXZYd8t0xG6zG*SddA_n^<+fvg55K&#$g>6lrE6;GwhHx*MH+7+Gw6 zR#pTdD-CZzJky-OG()>&{^il4AVXERzI3(8hZsSD*OyR~V)XxDGyd!Si--)EI-2My zOB+K114ToZtz*|~-u{>t`???@KL`zS2rSuF<3*x7UfSAA3{%U{xnj#)aM>Akay|^o z5D+l=(~su$IaDSU`Yx-@2h=fN4YsJ6@Wdd{TI~jNp|xpBofG>`ar4T&63XA&+kbk* z76q#}Z`S|#V3E*&`mtC!J8pIV)U?{ZxW<(eVF3e#nF#ahJ`s64dyUW9$dm>xF}3e? zrM45QSaOo+A7chGv^%n1mp;fM&WnS7-JNYNsQ?s->t3Ia(9QL3y&>0}s6*GLWl5W1 z(5rb|Mw&U1wg!fpch$2D@#gf@td-wSwvpweS~J4`1?7ErSnmw-i~R`@Otm!g@Kk;e z(`Opf#Y{e4%>eo=#($V^!a$-CEC%qT_aZ_xifp}=IBS+ zBKgpF#JtMt$B&J8%{(S+PUr0ZUWd2qx~0Wlf61&g@_Ot|rK^3}4rKipf`BH*%U~E&-(BlmS-xq95vg<(XtEbC9@~XYoCrxwlF7xEv9d@&% zUmh{|=$mk*p;{!(U=)nr3|PM71G2a<{?!GxivU-;HJZ)O`=79ZS68_E>fcvUMOn+& z8$vsXuu4?mhSWvfHEbNSfA6qlxN;KQf$OZaD#d@|4mj+@y|vYnIRm&Z_wohGR)8Pkbt#pdnVA0{XxFu1$}U-jNYxIr(#qlVzYo&4 zyGY|l=o-WBj03OE?X2%vRq({%){PfH5;H>c-+c_MyJG>y<3+tHbA{wvA5GQe-%YD= zPcZa5u5QM#4U@OH_$&hA0_g<9tqfGLhkEq`!);YRerEsCXHLU+pERjA%3*U2dJV1B zaXQIq2@SMfx(-E^^4!@NV_cJQI@);+V^{5G4dVPll;#j^Rh%8%17SvQ%fcK^Ofm#- z)ABMa{G*Hte2{}%CJ*+JdgOZIcNO*L(bP@WM>y^fX}+_%-^3U5L8tI=O!>B6D0s%> z<6m&}0DSmwMdHEcuKu)Ct6Mv-FKZ8S=Z7eX35F|1JzXQsor*aHL-pDU6OcJg4)(&c zk73fv23uqXAS4S3x3(yv-OVN3tWwc)H87>&qx9yezUVpTe+Txx7Dh}Ui$O(c8%St;acUHkLYGAnX(w$!~8oV%oQc4 z{F16YLFvD#{I);bu$*0n0T`nB(EHV7&F9+Q7ld4Fdq zJ!~^oqm&}mw>I1B$JI{BkkJ_69t)-=H`okn2g`S$9gp+;b?xp&Ea;{H3 zYPMWkN7DD(NP%rWXXee>Cgsn%s@xPO-;YZ zHHznMKD78-qsGQ%e9&93d#obbQbV?not~@_S&2QYt;Kd56=ir)g&cHRv z4Coc#nNiJtV($5eU5pG!d;|5zvxzwI?_=LZZ1Ir}*%DNLMV!f&6)W#QC z27ZM$tJJt4h#)!+KzJESndb*moyRN8n-9%!oprRFMUB?P^jo^4nTsMSM7>vqRNHRZ z^QL6~{U<9TomM1X&uAJB08wuoWaBYGa+LjrQf?8q!u{bbgtcuaflOBde=kXhs-+aQ3 z)90(ooWkivD&eqGE67lO!2e|5F4pE=^Nl10Gi+_286sL5kn38J@qe2$9!ski%q7bcQMxkl*&r6S}?ynT>YT}rS5ze2E^1*=3d0RDr-58x!e zkIas^X*sx2CWCvYiZ69-rbnv%=qHcM^iCZMMK9~bdxu2@<}9C0T^{|x5Hp()g6v7EJHg&yyLg z>s;2VYE*MxUktnBt;$5U^3=c{UwLDks|;)1J5&1!Zc>;m;Qjk}*g&V;$X`8KY0;g` zvdw;=++{Uv)$^G84u~JDkfZs+%>>#8kYS7{e98mWROU`fCN_%3@?aBMr$LDYXj zNZY*Zu%*-Yz;sczm~%8Sea0_^?7*#mq29J|o_%Dq8($@1qbI6QFBeGMVR+@f_fUAg z7oNLt^cT#li5V)`;TAT(zue2?JF8&QZ}{A+OvI?P&l_Jl+BQfZ%iR`FBNO(Ok;c4R zSMwz%5gVn8p5_C((bd6CZ>ay_L(`ls-PeHJ!DD$3M7rdIR8eo)OO1)}UBT*z{)j9j zy+~Zh@rTdgM%kK&*o!#XQlE2De)=vu6%Ca1+NC(5wDA)xyrVJCQz@I|e=%?FNaK+5 zF`rkyNWKjFJ6kCt{2o;$Oju00{dc6Ld}Ry4puh=^{noj1IjGLB}sJ8%S{4)&`ow3~otldX*feRlkdv zH($y&1>KLCtgWqUOnP1bJw?`Vrl6*L8W-Af#0j7lTTR7qm^l6i`8HW^s|f^gTD={M z?Te1*cixuGmyX#ybRI=NdUou^OgB9@3-})Ku8(vD7$EasgQ1&1a#QLHdX-6b3A_@P z@`Z*57H5mmJ8c41eI~lEsd1(mWQ?OqFFjSV1=J^}rp!AacO2%Oul7v#wTl#qbq`?_ zencb=e?#iF*t&2Jp5Lqg@+f@`CTEETEwz4XIR5iKS;72B31x+L_t`~s&kBVCa)eeU z6%q_2@!=@B!*IkNoPvIgt^3qsbf7a@iBgPs_KYABbfQ@xulsD(f9q?f$FFeuhBi8x z<2st|3=0xuY#d?7ilk#R-baGX%DXvj83abC$bKNyowwE%vd)$r%X(kdY*%S@a$oyW zsz*`-`ah|K|9wq3<-kdZEI8{3LM1HbYs_bNzGc4V{WQ~5D1&XiToiM)=lnn5n?>yZ z1>Y?62ctPn?q=rnOs6n+R}EME4w##66BIV>Op~8N`L8#IScgWl1@Fa4;2-Hcb{Us@i-18BVjeY; z%J*M<8u`IP^D#sX$jN0a*J(`AH1Lw^jiq1>&h_E=oJ!X0)(_v~KGaxSG}y~bYQCX@ zkCe)P^v#a-b(DI|6<@0yqvnTZAICpz>2@fuzi|d5{RasbxaR9@0%XahybC6)MnMsQ z%-(>n_Cz;7c^3JHt>ON~H(DPYQ7`x7=Ad%>q>VQQD}Jcy7-Eua@MVk_wQW998kXE& z(Q*jOP98_oTn(!oD%d7!!^N(G}|F5Yrh5h%ibk?8*9+S=C0@{232XW zbCw~BX+J|UeX}w__vzCx4@KVumO%CA2@!2e@r(Zs_xPWdfHoJd_e8l-#WZ-$jrRAg z#Yo10@69TOb3?B0eTlEacio%g^vS=x8w$zl-YRJ|*})UQ)ajErP;n__xnNQcxU!~S$6+w|`HZ4D+ntp%d~?sD^#8OxcRx?N(`ZTM33c=iR;8;ZQ= z=4jDdUC>A3|L2tOuwBZn{LJ*73RH?Az#(z&af?RG*j1%{CUVY+OEltFRz6Tqm&Ad_ z9L}Hk9jBDU6y#v&*A}_8HN~KLHl6cB%1_dW=Y9TV=Z@m9K@KCrQ0jV<3S}p#TUSbu%pR?@@S_dKB6i>a#NUC2Rdf|6Oz2 z&T(_$mpvz-j)*^ZhQ*59TQv{<7lLJVw$YQ`{jh>9@>Z|`lxlFZypv#Noo?_>xZ<~G z`5EdJaaJJ*_a`Sb%0s?S4E%JoJevClA>KQKA@_oFoOdIQ+($iI;mVNoM?WM52`q%UpRX3-UF& z&)ELAF{@tz3FpkQx@)P?I-+-=W)M|==9fCu#JLllq5lodl1e(q2Xa!rkK377#;mPY zzH6yW>hfWvyn@z6=PXsMDeVwAB$DU(rdtS{Z?Nk(zaxxHyF zQYH?&u~)PMY~i&GX8dKp%`7X`ej`d&Q$wsbfkOe$VA7NN-qHky{&_TG8vW+3V_$?s zDdfQeY0&EKbpaHtm`8Eu=o@~`pt@9m`S8oOUmxMjfFX-)YCdH#mn}eb~-@o3*5vx{cWgnai5~1?0B{X;u8_&lh9s|gXQpn8l|qQmZMkE=`Yb^smP&QqH3kOZ2FB~ z8eqii^kG$BRv=BTGthb|&}vBwgyWpfdUYR(afSEioWkD$tB}hMjcS{LuV7$R@!MvS z 2>Mw2QgN)}z=?rUX(6FDMqY;Vx?s^Xl`kG#ze|^qwcBu|#^t-b(Urh`~)!hba zxj}g+r9=dztUvn?af0hJjK2wx)r<$v_h{!*kgP96YvpR|Ib zzc0Z%=4xH5?ya;cgEkVob4cd+EohW`oNdgKJPZ!8rpi$GX;t;F+A!@gsARu?LzCsX zoz8KeAG_&5vp@GseMqNj5pkOW5m9M40B*=~xVwgB$#G8KsU-cQ`{|mvDn!xIdcLAi zZw0C~$;OiUO2r@E zK#dcM!kZ@fy~$DTvLpOtXmUoOaOBLc`>ywSLa|rS@>q`%Ldc%oN4onFEUoiACd$*6 zF8a*0lfjRscl!skXgHlyh9uqD;hKNv&vKpR64;SrY*V{)I(Q&OR2h z%~-Yd^tVY>i>7#dF?kGj11`<{G@k+1`>VN+?BE0KHb+yd%;dj_MZ`^fXOuA_vM5o< za%gDV!-hsJG^g+*rILlTOBu6UVYo znb|m`1B9NxIEEs;Kp)?yFU% z?4QB)jp66X+VZnERGXd=oD27k3V~7Gn7A{WWrr1g{+EOWfM0Rxa2YMm$+=-b{Z|*4 z+th#CfYTQ&0r9&Xz{tH39eMt=&6>e_;h%2Fe4;vUZ0>hl`l zz_U%c@-}Rd#%daaa-iBiH?Dfh{Qnh1y&q6P06zN;R0<~En_Vg(Ks&Ym+EQQA2MOnQ z&3p3rS?y%iU%6sEepjoQp??q&nW_$#Yuyhco!%84ATkAqcvQQyw39Yf<(%wI{Yn#zCa=>xC6szHtp7PU0VA~x9E~kP@2&Nhssyy5hj=hJa&0>d!JBmM{Q#!oto-d-;*Dg= z%@?Rn5hbcr{Dq3JXb7Ar49u9pVg6OfVhMYMTSEHIX6vsx&wRJtQWF23qx}N7mJYf046K9yxD71aA)A5!}SxV!A1bzD&PgbxYQJ z+(AiRYI2yvNtRDB^ci#j(M}Me*?&|+np z#&wBSO^<@#?`JKa3y|+D$NY{nB!!T`?)A3wqpj>oify+`6xmzC60P_riQGmw*oZNu z{}PCS@`928VfO~;ih%$xwEM^x%5{dnSI~O5IHKB}EyqTk*b|FYQbeDgBvSwb)Yf29 zZ7zwrZGSvg#y}H;TYnPBZEzsz6ZxuGyBKD-e*5L#lw^6+#Wz8-vjoFJ`>EeG){ho@ z&Y*2x`ZObOKcv3Ku+cs{Yqcct6|;t-0Z-!fhD322ard76(v6VE z^P^MiS)}388|U?J&BqA}Bb$wgu$x8=SY_*VhM-xhWsYZ2^wTa4|5C&~z*r_sQ21x< zd5Ev9J>1>d_a-EpzF#PoJ~0|U1kCmQs^$+!-n@L0Vagt6`pgG;aaZk>X*WutWjh7$ z@WtC(=q4;(C}6_LKO1$He5aa5*0VUNxaVTMF(`4b=7i2g1A*{+d7xk}OxxjAvTw*( zbpo`7T97z>DS7P(=&E}7$*R_ZR*$+0a8Tzs` z-4+ulCZWxXk^U>!0D!my@;)Wl^KsL(_Ec0jp=uUs2d})d^#Q73?MaciCN#CSyWaBS zMj-&z+|<0l>1!V+GW9)it#_`eK7XE4bOKmXfP6W+!O;Umf3uMoM<6mPN}5h8spTv0=`254$K1*z$` zB=9?cLag*BiwLl^z19o1TY+dR3m$O+HI!w|G%xs>ZI@%?&KLAD{uAH_+#=%?cgNy% zbg$_O!y+{U&H_~c8ZjdTB9tz|LNFq6&YDt)Zh=QWM{v{oxh+Kx=9=z>^jGi!>`X1? zL~WMGtuXth+l?qhMuAy~G1y{+t{$jCFu2ridlr zcFFSgj3pP|EUj#65aE;qYda2>`?Mf~^9&x>~oI&T~+>+b50M!y9S% zN7{4Y4sXqbDU*yMiA3T$AqZf45&-xMOsA>p!w(aIm0g9$A}`wZp8v%&2a~dtQ}Io- zizL)sx)=}dv2;EiSv{_5*A-deKj*eE1~!)5x2^i;3<{M-6hz4EhjZK*P^C+RS$xIB zO^}r*=iri!NJ1f2l|{tEqyF@cn4HR*4d|B&?oaCLK|9Ot;m*fCC%UUQO(%-pC$%>M z7YnN`t=dI8D;1n&eU80j9`ntst5ulPqAOn}3*=ObmB~tgg!$6_OkPk5@WJaR-klB$ zId4bX>Q-^?a@{_`PFyWJQbhz_Jsw%^!_?>=51iSN%dAhp(3eLe*7}4McjiMW13y3I z>q%*YuknYrHZsJDUsPbk-R6N&6g|V*-TjYyILOO%zfcE!3I}-i8BxA>-#N!~1Iv0N zdL2m2aE?d>#LDwIjy5y-9Dgi%?qpXKIP_|@`ZPri_dWE-)95cQTMi~O77IFwZ#U~w zUOBRM&2ePO-~WpL-WN-ey42(hlOME`3P8hwrxrtsfWxIkkfJ=sVZ*zdueD621@A(` zlY8|=KQjku2r{3e1u((S9~>X?`3$qY(=5soTFGyllgAW0s!3&|omC{7UbJ0B}@cNW&S z3$DgpQ{st#r2mDa3Mge;JGuTT`!l5&5Y3^acQg@QfmgkhO7Kn;@Krzxc8W3ADDVpv zP)AA!o(PxS7UGNgkpf2!VejiQ#Y`Th>%&DFV5MgAvgor+UEpR=mQ<_H9mh2MR#(>r zu-+mW*j1zdH;K3DYSvlR9(cL&aoqmWl;q5CV^exfPJ6hBMFw$c0pTpuu)-bA-Jw`;p7O6w(gFQEW3)G%#z|NGZe_ucGO_JY?6Q%E6q&6likDUL%1& zBgc%7tvSt-e0vO-um5Jzd%c>BqmnED zIxYcvob&HA_y$Lm-OVn0KQ27(!+pRx@dP*DfyZ)iD+S&$x||ZC%?p1^LMdKux_;(`L%yYnsc&JgsRUkS!! zS?uF`R+wf$M5iuLPvjkZ<&&#&9E^klcIWXixM-qw5lDLD{hIdVZ&%w6PDEAdO33-w z*0zqtpTMN@uT1Rw{6U~wF9=?6N#*RY!_QGPWStbH`g`2y0#zRGImO?%>3?3W2xp{r zpdLhW63qAzIQQvi*oxn%#!1KJpp9S9PnCa6m+DFx39IMKQIv8gc)N{C#M z63CH=h1|vOV>PQkh*huxX7l5=XFujz_@C&`G<=o$~AK53)a_&rTVOwslX+6i&Bzm%T+F=@`hORpNkV zqd5rzJ5Q#ySN&ji@aZux+lb|B9EQ`_&!ZFQyYK&6YrMCEjjk;L2U{wz2jvqm6evdb zSYmVp0l_f8$+ytX$tOj?q7v6A0(am!1J@b#3K+_@<3>079k76ddmh-m(C4(|j3a`prSkNqicjKP(8`LE2OH^A6v_a%DEM3*S&DzE@5 z^WFlF?SqUtupwv**zhz<)(Y%tnaIwHL+7Xw-M#E~M!a!dk~#TwmiCd=6ad5v3yLiv zbD4Hoc>i-;xg8$$(j@sF3%OSw8KXp;2?%|4ws{o^JRHnZ{iz-{rfjar z>DWKeb4n=d*pbsgSA$3$=K(Jwuqu7>DX{vzDQXO-*tgIi8ZJ1o;_6h+Kk{F0&I}r z0G2--X=9sUZwjC>;&=k9=%(*7dF*}(D+J=Bv3o>!X0FDW3dG;#4C_MAs~m>!o_6h>MI09|taTq@f|bf+_lJM@!tBQ6d1wv!^P9T{f~_hIl^ zoMQB5q0GOqFdWV#4g>~eR+S$lIe<9aPu5)Xz%pbDmQY&=6`YW9oO^%_gz5w}g>08u z;m<4-Jjp1^=iw-XNGxZ(DPH4b`NE8REfO1#6v7B}w^<@T zih>fE^*2KUkjLx8`BMR{G=f3@9g!*Buo?VAW;~YO1f#=OlO#bz?y z#LxM}^?$3U|GMwutk41)`(OgHkF*b*hkjL9N*W=kK>gHSy3&6j_7W+CrT{Gqk3PQA z^Y9qA;FU`b*19d%iK+NXU=tnAT4T!# z$%+{Tet8Ehd>k9p{P|HFU;OlQI41TXusUiI30!Gc^wHysgdnlOIsI+^{I>O1ujKO2 z8uA41O38ngn6+<=OE<}zZdMqq*_gjcvkkH_ZS#?Cc5`~UnC-?)0zZY$AeOH&E zNHZ)}L%_L>ORA;O7HM&$Wm;L^aXF};H+$0UoDB;X4H*GE_je-Z?Fr4f*ob1?H=!a$ z(PrvzX~!TJ*P`kH7H?ql=iFZ%9IUMXNd5Rh%cEpC{IRXaJ6`jE0>9n7@J(RB%y8l{SD3;z}ehFw0S-^aQf=RTt5Jp@O`hZOv2`oAL+35KTdm-u;sU4pNbG!+?%Zgrzu_||S z(=7EyIEA(Okb~Ja?9zi^osYOnmKABM5chvyLQ;}&-&nVc&E3Yu%HY@~qg0UAu+aGt z6{+5>$0$aQ&3-7F4U4P6yRD9(Lt{Z{yYg4oLif?b?y#~usA(uj{QN-Z7Wa& zPM$1MCsi5_{#-*M%^6sR>WPhju_+LA-ibjGLS_ONpcQ0Lt1tOorw;onSd%q^UiO%% zas}{XS(m)Tv86l}i!;t_^KrYOh;iB7nDjrXy!Iq-Zh--^Tf&Rb-yK>%j{$O1AU9o_ImnO(WT|d!;G664&Q|(u^e|($R z(AM;cL`PQrZS(4YE=4AI0a!gKCgabB!8z|-W!*(8@*OY~&ciQit6Khp(w zqYfZ-5*XwUX!bPRxMq&p5y4INSzGphz^~)^^9?(T@bcv~ya!ySm{Txj4IXR2u ziJ+Qu_V+Hhki-eChD1W^@9HrEI&^Oiw~^UHA9|t)-90lF21u<$1kJ+*uqrTI$e}sb zJ0ytfu@mf1($V4uh&lC?TUH%3Mh3S*tdVpfc<@N|p*vncR)B=|7umrecfp`w)C(n? zFCQo;5G(!T9x0E25$CU#2jAoIluXaU;1hC0#$dQr@_3|HK$O&48jiv48HvClj`q zj@oH)vZF+6cV5oCTi9lfg?YWBB?$qQ1e) zUXUvP$Hy7PW~BJuwZ?R#2}kNGp6R|R`4^TejC0-82w>^)YSaD)tuj-gS#&VgOA|^QylNp$0-xK3yGW_G zWnm9L2fP+;?ICz0&gSVa$Hin?_-0yB163qM>-yqD{B}=647|aZC!7DcQGn(=fEVd| zg){jRT~NzU1am4PO)jjC=GOar+r5d};x}rW-f!)C&bKD(6@cWMh=+C4u>HG*nf(wa zUUXdoA;~G}BI`-Q`_Z%g9s1F%HsAsOCE) z^;3}`_eybwi~}`06w!#5i`W3}*OA^d{OxuJ1#nk-& z*&2d?E!iraSFjCzJMB0OI6RQl`z3MmuK}T({CW;ksGi?>?*&l7W0G-wFrPIIXGQGL zvtbYlXp!yCYO+Kx9eLhTTLH%_nLr)}IUn^@_xBjO)0+dS@Vc{g#&nsb;eaTTe=ovxnd_#|ISnAzJCRd`g|m=VKh` z2?7f{`oq3|XLH)q>MiEu!pFt! z%#A_Dor~|W{SuxS(-=8nQ+8vJ&mV8?&+9}x4s4+DBM;R8s!7kT|42cwv42MbnURFmBRgSEHEP>KBCGps~(D zlN9A}XE>QTJ*{@XM_0&_Na@u7{6^n-o`#g)?8H;pX31NC-Pqs#bdcLMTeMV)EXvNT zzfb0OR3w!YhGlu;MmjKut=f8BpSkMx&wh$|z`Ln@j;1@Y^mZ#3w%Uc#p3dUeX{5KI zBmz)nGS_bnjl%aH-_v1>L66Q)^zJZyjDoZ8Wxw2!G{J^R{cIlfAT{IU%15$_cS+nE zN!1~{ATMcSl?=^Ga|9Y(`ecRLFCx`GIwC7rYCG%;R8udjc&8z@m>wu(@M(u3&>ILe zmVv%$vS%1;LP72h@g^K|F&GPS2vTvp|M*1xk>L-|nD)COFSM{fSlaY6hJR^1G-SI0 zTNuaL*uIi!BAqk0+<WQP1@$-}CG{P}MlWf{1Ls(h*w zl@z*fBvA#sqZN|5wk^&-&1d^iq7!j{eBsK|5aE*1M1r&(#d5-1*G86|JS|TkCt__xShXWcN31uBcqdB?rw8ic^ry;+0IH9R+L@ zFFm>k9H}ftj^A$o-o~+-64p;*TwBb`^vyJ=Bv;{OaA`})>4kj(HsV;WQUdj(Ox7ZH z*oASJSgui7PIU%~~_Zw@=X=PmV(c0}9LQ?4QCsYDLJTJ18Pt(SptR8Y;$L_ewc}J(a8L1i;HCu^up24z)9rKilpy@c4 z#dKW`hU@OP9khh!-9JepTt1}GX2-ok9~44}q#+WvXrw)8Csd-270v#mcWm8s8jlQ5 zwOe!^TUb&rThg%4SDYd2b?0tzWj#lF9mGoWx0~mIO4_1?^So)?(r8;)5M2UQkf#s* zjH&QrfDgCSCRG*t`~}yn{p$4H8>K&M{gG`IaK?Z`-HT32dU;-D#UL5j()hSJSr_~K z@UA~WxaE%Y?$-Gaqg92bCYcXv+BMjz`c>W4wQ{sys#PCp=d}UB$;I2LGf<`0heyMJ zbnUoV%I0x4o795>O=mY`;ss65lUA9v$LY>UMQpD!U^8c6ubGvMX=18K)6e3yv@w&g zAyK{V`D-Uoi%K5D?v)Ze*A}`gxUAI?1CB3lXqgJr(fEeZ&X?<+Z&7|o6%rYvo#$%g z4$rg4c(^G{PxSYo<7 z_!Iu|bL@DD0D;|~4hawE{J<%Ud)-8)-e&E_;BjRc`|hq?R=oMZB*HA831R;2mOIVs z9d4_+sp>&PAx*JbLZrpcW~^+dJwQl)eO&*fK?FLgl*px!rjc}qi(Bs11UqS{mcbE) z(={QOSkx+Rs8OL|95a~^7PiZNShWJ{KfT&j6KQ^6F0B>cT!Ss{5##w93Ok(n3?8Gd za=jsP^^J!uH%>WvPDf^TO?|4`osce%#+(n*ouG4|xcf2OX4k!+Z8^KTKHLxJqns5Qfgc&$xlOc3yqnnrL0Nm8|vH7k=NI>oO_v;pi zoF+pcn4kaFb|S3b>uMlan0W}U8<}ZkKHaEL@8{SKC4dY5bZ6R9)IIs-^SnvebsOiT z9u-OiNvx~Fj?6vTtv);(cR?KCgZ{6E)faL$(+VCNV29=f)kMwKB4M= zkgL~=ZLL{Fa?kte+VOBg5ZZEVL&YxHeWKX+ehnjVEG}DpYHYcs9*#~HU8&ukldL7J zzlegs%GviOJq&{g5$!Q;N;Q!WG0*L>E+4w9*O3!3+fn%%XN@FU?{2RjQNdx5@IZLu zI!Fhy$DZ6s{kJyv?}$$V(uEC&g6K(n6HI%2G$I$mgn*ORRKRdp2YXgSgs|k)*bw80 zyGcrEl(5Fx*$m`gyWQg2K2W0UAT>E_-NMMITN4W|hz0843l0yt^kjct*KHr4UZn#!3;Fo(!_NcWJ@WNFCFbibpd*$?d^eQxeeP&{IOHh_ zM^8Uq37OElujAh}q&^g;|6YnRtW$oQ@Tu96cr%s4hz9B*voG>)9q^FxV6&W7^VJRm z<7XthT7dONl{yuq)kSd&3+f(4aYfqiuW4oPV*R;&h*dP-p6QZ)CF&^UK6WmO=w+R* zdzTeTD#WFV9U1+A%`qaU!Fo0M*~+WOUQ_j}h}u=|moOP7T#mVLLa{|d6!u2gTS%D2 zJ%#r*Qg+}O>0F-QtmJB1`N#E&GN%d`Dwq#K;~k*e3Iz|mH0_)qf6}2Fg!m#_D94;D zLvdf(|C8PQ?_b^+6~4zuEQx(PnVeg8WDhE)QVrH+ySzf7e!<9&5c$hz(<3B+kq!H7 z3VAv;$|8%+d@acvt4v(trzJ6RsZs}rx2P-{*)k|Mi||$s=SWQNX|t{ccL&IR#2z$X zD~e=WFE;DjZ4NQk&e?@@E!>FbQAtXQ9M2F08FuV=`y;QhW1Y8e{m_j#1)(7YyhKjx z_`N&F+P%dlVR3JHs`*a1M5g@4A+7S$305v};WDbm3A#D)dfwFSY^Kt4&pX6SvK15C z=p9v_@#)r@q#0ZiV#5cwIIOD_hb*2pm%RD2CL-0}T7k%}!6G5o42Gg7dhL-TqBc-x zOm`fc1-(sL%)b!d|MO@F;SZ^8+>wOiIc$$!ZAZ;?F++{>L<6c8JzW}NnUwj&@sKQX zZUbAROr%V&=C(&)P8DdVecz8I`Iu-i2)3C>u-2`$i5m8lmcXG zrVLR(WJ1{j!y6BpFOB4;tfTZ&j?`2e$wS-T{RYw6G%~_2_oUn( zg_#e#2%hb@&;^erC6D6(^`buf7q6cQoAbWjcTnnsL3syIiwpBWJD!)R=f2(#v-OHN zSla-mgWi)AhAo0C&@Ij8fc{p(`?`AXt+Q=IpX>^i6P&|QmIVQ>_YjhE1GpLW3&Tlx zGdEE%93(GhDPV6{_z=LMjdFQ1rgR$gax(n)K<4X^lT0`B49=(j#~4L~VB><67-2ZN zcz?sF>vX#}_GDBMvB==^*G|)Uvs=~<>HEB)5n06HPVNOaaatka5fj{RAlF_oI+|(m zBw}_Ix(+mW)1F8Q-(wokog}g)GIkLqMK&@>#A}yLYa%jtwml|=!YENcEOaez;IjOP zUcbKTEb<;K6knS*Pyb!t^Ii6;D_A>Nx^R-~q+S#m@1@!caTLTpu3gXDQ%!hQ3j}(; zY3pwmID!{e^YxCWJ%IpMSs0Tez3Sm%NW}b(UZ{s$=3gQxDd(^~+DTOfxtI5rk4$@* zoSIXw`+j+f6GFzQjE6K_8^V9}jy6K=71pvO90r#0U+#|Q#k_L2&QNQvsW>|$2izI0 z753h9st=!9Zk^__r@-QKg{^RMVWIXP&kI~78c2{5J1Z&iq}}}4-^z7}B9qN$6=lD}&8L40Sg ztU5;=u_sJfJLewao#FvB2GixApN)qyz+t}~E=FP~Z32jSd1v%T)nf$xq2@topF%dJ zd3ugTl0sdfkKKu-wI>AjCo3G2^PrKZfV8jO|Jo^tI85k(?!5I&w0pKy8bb6Bzz-Sbx;m`<(iv6Y2t7 z6P?o46wjOGktfC&8WpB8@Nb_~V3{%lt!lOUwu{)h?hA`Q6@t@GAaFp21;+Ta6JQz`>vNyG!Di%}Ay z-<>R-q|#n+KUSDL6EUyK4y6p8^r{w8m4Bp~OL5xdh@R+h#M8-+OD&W}VbL^Z9MHk7 zm5E3`e5agrUH!54V~RD{a5a&30jj)lL$BI`$U8p0pQ|gMHwLOU)_-U&|N28jo&pZk zlHa|#m&N79-(pv*_H!m{JeJwI58MJX&xtQ{gc~5uNqh_`XF)L;eNZVJsTtPyU(MY9 zOfyeqNFyKKwOBwlnS=Lu5uWff9;0sz#5aK!ReB3X%s$FHMOOq)T$M}-v>$(JW=~*% zA3J%Tt(^TT%5QO|?GPB>q76MSU#CCW-}bt@H2-7qgqMs&F7kMPjT~Vio>|wxP?NR= z&C|m7ZPMx2BuCmXfH2r8m%^*3A6vU41xpH`>k0F4{Tjxcke8xEpaw(4tcmT0kTGAt zKt*?lZN;-x0vAss3DA@$AZ}c2KP<2-ez^y8@|@=VxP?jvGMC{w<#y|`f_#Y2KA=V` zQ2hn&{_CQ{ia>=lL&g<{ES)^`f4nfvh^&E^K$NzKWQI_Jqzz@R`k020AiZdScU>56 zufGi$JJ!cv9l)8Ae^ynhOkuLw2~uNb;8gQ9%KybPlqT3v$XBde8xKAU)Lb7Tjv0z_ z6B^i?_@gc%_lq(nqJ7t5*ibA;*|M-mLP_@(*qLVaaa3d))YQ~ek^YM@ep}IT^7&T!YcYLY| z4G4wDUM;;nte2=SGTM6W3i5l7sucDQq1JNb%s&7G(0Vxd$$`S~s*i!%PJE}5Ygr^N z8=FA*?%!(VzxRE@WP=+Kg52iz#(B#}s_&WL1*4=k0l&jmWZLIOYynGbBxfKzC)IIU z6paCZ3zjUU@!CtWeE2E5wmKu;Itv&!t;#LvUJN`Ndz-P_XW>j-qDvfF`AG1^^Z_jK z>S2xP#9(a`A|_h#xs)_hMhqtCI+N-SLU*{M`LaBxUvKx_(=*e4a`JtNmIeC>Ok7)O zx0-~(%2e>be0={gD|n+yJb_2*UHU~4b`88p@KXd3?%i`BHW1p-={bk9QneFXTQ5;zA8+>2ZJWbvjq8fimXFir-(nVZw?GJ%VjP!zn z(F_Tm?d>6DI-8kn0KwA*nECHhC&Apnr^LC4(k2~9l8Zkfot z``>3#bp8?G7W;8v)DLrVM_|N1pqL0(a~S{ikMDq~*@Fe|TC4w$`-7AUi8KW?g;+RR z&p`*2$-5 zD7X7@7{c2IFyWJ`O&U^Blp;QLa@6mccV1)!8qY6yD4jx5HjNQFnu8-mhj04}L&;(ME4gA&5MJ^fz--A5 z6C%e}%yU((Os6{?nH!gnRD2M?K-AtgR@zf6U!t{N!>2_QC#Qr&_?})Qo+F=Eusp*G za1L54j27q9Y4tGFQ~ZczbHM9%_EC)pjb3>7T2Ad{Tk2oY5mpT9L*NS{oQ7mPfNzT@ z|3`E*LfB;}YdY2s)5>`i!N2tm@tTv~JHpYD?D(zqp!Ei8CyGFy|Lbc+GQj)X&(Od` z*C0(%5x>HgS|Taoo_PTXPsnlU@du9T5f;)ZIN}ljuk|Eb%f>^*z~4-FgrQ8KxQ6h=49Y6399J+T_{@y>H(n0<|kT@m6 z(uqV~lfsw$eB7j376Q$)T@+;>b_yCB#wT+o10Im))u1 zvyX4!+7*InMxfHFLx!4R{up}w(kdKAzXNO2Rc-tcLX2(@R@lO#YlYXkr zPr0`rvwcJ5O@Q@hw)VmOIpVcKpy+cPzbUU-$U@>ViYju!~v1YQfp=Wd%N4NIK34l|I%F8%6OnWdt5BdLoi^`J=N2V*}VWQ{i##H1EFpgLI%g969{3 zPfFmkjb6;i`-m71J_~jM?=WW;RHKerIE%jBezcXLv4HEe@ZH}sDBq9q{J8^<6ua#a zHZ#XIC0s1=wYJ~mnErv7=b^b*hlX2_(2{&6exY=IQaEhAZ^XXI8Rsl%v>r4IX|Ygsrb-DClqhIK=>&erZt z?QnOW3sT^m24DRnPb3xqC+$wabMs$Bbj$5oGKa+|^~YzpkVm7Ext+1j#t=s_GAw5Z z`_k?1I16R#_nPK04;GJiNa*yRF=zx1LlW8$eW5JbxuYVGWp5-yzER(;lTsb8RJ|2`^ zL4moWKCe_>XSWi8@UB7&Fl}CV>ZpH79!Gi2eor}7g=fF(beWl- z+AG~RGqoQ}3%9~U+?twtTk7(jY_gR+ITxwkxaka{)e{F90xZ7#_ka6oeOyFZYt+mI zIB2_eP^IW~OtDztZQ7O@=)U#1!21xuL9yhJRWQ@O;ks%bYBOT_K=$3SNtz#v+$l!~ zHJ^1_`A~V=kINbn8u!PdZpTYQF;;cTCpyP{#_UsHZK+j;V2*mgmg$JLm02{997X$Gc8-u0C5j3tNn3^B~m};e1R!_`M18^{(h2wIkzCk(^kVWlD!#^~ny7 z{GT&vO+6>9RP8ZhG`#&$q}8PNL9>v`WTq*$yo5D>^3#y0n#0Ci;?)iNIS*ZhAd5|gy%hPMCQ

    Qqsy`S;3yx?+M0sk* z{Ii;*`FD~XA@Kx_bJukD9DLz=$Fs?y&feNG~G0}+?8lPqH zJ?wmwwN~64Bd*WU|IY*VF={Is`C^w2JNbxB`+A&pBq}aFL4lnzv$dJ1s@uiaj3m9s zUwbsy({Hbf&`CMfWH%VL9SxDm6-~%}l%v|5bn2nIv=$?WT>LWa@wBnojxS!KK~s&g zOT^v;+Qj=PK6X*1szLTVD*AqaMCY=G&!on4sHjTEZy(8}A>+;f#k89bJwfGQX+V@d zff5$C)*cL}F#^w2>`V1RzVB>fe?_h$NSR++0V7Hc+FpQXEh=(iwJY{{BAZo`p6hl_ zO~0V)Nj`~yi}HiHG6H%faFq|X9;)ih;pq$YnXK2T#9S;Lw5`$+TBO=wUu94}ypgVj zI6*ngXp6{0;3!!qs@tj1o;OZZinS`Z?6fnG zQ#0~0FTH)LB0ORBv!7Wm0fpQ_ZIE7;Z9TcPC0=ziJv!qyaC*oR<}(hmY{d-Z6Q#HN z{0}Cp9NQS$t4AH7eEh)h;{L-9q(k!LWIq(Bl^V>)4G%1R-T;hzlq>gSTd6C2d)XS;2Q!9rb6s2)K-QTJ#^{4j>iHJSo^jkiKQrK zSRSYJZ4N`ihs#tgno~nEVX!gu-t!U&0y*;@=e^0GpbXRL&(66vn>#386837Ivymgc zJleH_^aT+Di7>g6Q+Q2 zSeWP0>xXQSz<5)`Lsyv48!P>?_fgY(&L2*j1067i9;M)Cj5eL?!egR|tP$UOU?#WM z@2GwzT~Go|Q^XUuWj23(($C0!{tu5}`qRMU<66s!9>6(Kn-8Uo0%xC~-IS?Lx$);B zmBr4^xCDWeK!6lX0qIsD?a}{HMbV5`Ee$YyA7D+|#~XaaDJ?HGzf&Fy-)X_thnYdD z&n@7>=pr7}dSpEiSTVT@gC%PX)-7>i1@aD9rmENFM3Nkusza6!ed-7W@@wpi1ylF+ z2Q8DQKLv_Df8Zw>2F>y`=^kf^27&cDRykJ_OnP-?H)YVuJyi@VD>cF6wO$j2pO>gq zaNkoyrG$Rs27R7ydqR{n=veSOYGwz&$R~8Z(a~PI@!GHW$#EdQkC8z1ykLleLbYad zG--K~@UXGJs{r+TO9n=t|z99s3s1Fx_luJDzI;2)x;x3!Y1Z5W1dO zVxo4X+Q&d2#GJQTlD?KD7c>c5iav!5>TEJ>@`J1a`ySaj_H1e$-C`Vwi{b8f(Czt# z+K3rjoWC*dtz-y-zqvpD;d88-y8y0+tlks0K8k?^pBqp0u(htOLr^f$OZht9*<}en z)y)5D!Ci{ie);+k?8Jyy0%k1SMPKWw_Lqz#s7I-x(8#hbSHYXI_ytk(pByv^l`F@? zWJ6H9(C{wz@&1+V5z^_ zG-4+&(@@NYq{uI09c`4UW4Z_s+FYsKV6V6oPM+UPB$$_jyGr$4OS8v1m7@@C0&{7v zyMT0suB(mLLb)8#*P(T}Dl3TZ#5jb2&|&EnWb)%`|?inN5z7}VD;p?zChQ#xgV+`xtedtFq+Y+?_j^v`(f(o z{S#rj>w{?4GFbsnJ9{C>OH$;Z`md!0=>PT`SMiXP)?15N#@mRHD8Z<0KhK zY>MX&3!ZbF)E_NZrK^-xYEu__a9idy>n_bN?nX&v>rGeKsemkP2!6tT!7AKh{$XZ! zV}oc3UPaNAhA4C?;$D;Yk-_yrJ~k3=X|qle14{oyvCfYkcdUyG!TUM;{m$)mesw`s zUou5|((xcild10R;|;WC&G}uG0yX-keIPlo^|H$|sO3Yl;sh|8e}vF)^15u{GwN`N z6EXRd!`qw()GGaKgZp8w;qP@mbiNca;9xNGgCz2&4~oZ&;0K@gcI%HA1Fx|{0r79A zX3bNg5UmJPrL@jm$28wX*JG@+&Eiltd}f0pnalq1!D#}4gzFc+iv(*DpEM2ne-Q_A z-z5}kBq}5YNhsM<0}}J46y=hYz$0*WU^dzLDZG+C_?B>giEn@vy5%Bzde)Nbx+hw3 z4IAq2AL9I78W$3xM|rBU+q)@S$(-(5KHcKgqQpji30oU1l)0bT>FiV$YFug+&y0qZ zXk|+~DLHNI5zhOB?4nbZ+ocWCgza*Tv?B}VoY(A6=vv1acYfJ^o5)SybdwiL=Xs4m z>HrI^6mR zp+}z1>}wUIJ$E>jDX#SU%A3b1P4C>)t1RAY^0F2xdcN4*&Bl-?CPeZ*l0IA44xO<~ zO!Xx+b84*aORr^f^-CvW5fM@|xLjxqXH9vm!fvmQ$vBW2BUq2&D*lt@|J2!xxuNq!SaJ|oW$nR*i>qpN`+K0L|RYVl&CP{7?pUg+u zj(HOmj^}tkC@&v#5@mdTuw%4Pq3C_?Zbg^ul`!dOjc3$dEBsw(HmYmg3HP`N;jb4# zux~4k?Bn0T^De7l6>qT54c(7E3!D$IS7;9^x!*b#`=qN{3@NFy@yo>Xe9$uQX8Ayh zY*qEGzk?+y3w{Qk1qdcD<<5j6GOKjI| zAROWTI|rC9N8_LeP67@V{Av`sF&SBJIs+RPejIb9E1qFNfNqB7Zx`xhcbKT{zii#lR(s6HvhQgt81dTQD%7GDndC+iWUv$$q@=? zpJhBB^3 z8jf{(`FwW&z+}9qfYa8>C&(^%GkyMI-W}_p3?@rNS4+8%ZG(4&a>QSh9U@voRH$5@ z`BAH7;nlmZL+bA6b=0J$HOdsEYWPS(BpWO3VQ-Q4B#e+HL<2U)bH2!<)%wJGBXgscalU-aiEeNa86aYW z=Cb0fA#TR<99fX`fPUWWJ#)jXRp06u(KZoTdd370Hs}&efQaU^*FiNMZeD)Y=lsNT zw{X3=&wr8?Rl&=k+q&S}3)GdBPkC|pCx5JF-v6xrG+)1;++GoC$uS3StlTX_S z%g6(ECtf9wrlQ7ElM#sc#N`50h3&ee$FN74r6yE!8;&wkarx(cnB;U3>e)h*>vlLB z(#L={e)xLtI?aYnac^@MM(>->bFx4)2)$Cc67qo|h%ev{L57M1Dq#+w$*46<+FgoO=aao5>zn-80v(ZFkm|(<&vU zI_mt6UPUBdDcWx$qtULec1x>^{(PWm2#wkdN=4mN*7-;QXMRGBi^z`lyW))Kb7c~w z*ZR{h5fwFSylg=-Rk|0hsbuOMo^=BStbfSl?E@%vx(Xl|?dq*x+;B52XfSkM-kROq zOm}=Q{m_f-wOuE8gk`f5wYRSwDpI*m)(h^|E3rY%MDoV_=Kfs*Gf2;i#_;nOX!J2&)db{6B)!vqwq<(W1Li=m24-719jt5oBY(#rCsncWY zD*WfP&S|%&FW;tmgw7ZyjaCJwU`l49zsXX-d!<7yc|Kf^x!cIO7@d>ed!gCY+wS4o zyWr<$dK^D!cQ&BSTeta5&&iga@bE%)h5_+1uaF}C_WE>e80Y;Ufj;$ZLGF9t=7q=|k_xZs+udcz23*TjBQp z^FdUa=@`w<0Kbn`7$GMgfpu<;d)*!VkGu4sP79r8LEXoNkGm1V^;~Tf`$1PVPPLm- z;SPp0%=EB@nFc1hj;ltrm^q)1N*cuywH7mj->f%b6((V$>U9^p$?RV@1?16l zM6N3EwKXZ|VF)8M3xo_VOy-rwL@eAYe-Y4B156u;QSTJW^BZ)>#Nj!L|CS3~Ff*yc zba<0xhULEgV>6w*u$_;S^phkPZ4tlfXSxr{3R!v9hk6?C6M;ED7)^ydE8&3|6{24d~w?xhw-I7ov+PWgYryQF^RKh?hE=;YvTTlRe&OQOvBJy}Il*3jwmK0$kR%q(MWN2>Dz zeQmzCY@Ql%$_Lh@+wPwnNj}z6`4~=06H1QJ|DX*=Ny6QPFh4n&#WZc9LsTM`9(bc< zW~*-uf!jwi74npU92o8U%jPFP)($z3MR^Cf0<)k|nB;*f&*00#RZC$=_@N*7mi+J8 zv!G~a5r>`bxH$yRg#n_&30Afs%SW0}gecvp$})Rf;JqL|zemMjGDH zwtpv6CF9}X`e1UVY`>2gZ97ILLslGIL!54QKi#Y_KDE3ZMe_+tXuMUTZ&sYmL>dWH4NQI9Fas=Be zd!N7HNO(#tYgBYlden%0c{G7H!Oey$qTYRTr?lODkND?;Rw2p%^RGZ5kBaDBktH!_ zOv#49>gdFJ6tQhYZL?>GwuY}Q7TQI`AU#$EqhSG_Rdno*u&SrKH@|VCu@8p5453M` zwAZQQle(mJLN!>2&O=khjC{iaQ0i;`JomVRVtGTT@ASK-1z&m;C>8GE1c|VVoXLAV z9zGoLV7an7onyc8zywa|J0ro#!y857=zgkLMadW z4Yh@evWksQ1 z$#`=M1u-D)A=={RUlta&c-K1Z0)pu)vnMu7TIW6^oQ(FA=%KEqr?r{RSjUPxHR3|j znR(~8q^+qA=I3cwiYEi2@ywd0&9#b3_VPCh#YjSSrPT zkW!qN%&NO>OYRertrmx~E>aB9@YX(YIM4VN%1<@C#CJ^iB_%?Ic@{QNX|30%px8f~ zMh)}wYGH$}tGt*8o>&*Z=4xe6zdG!z?h!-J8B*P(H$G}mAx2`FwJs2`cM3j90NL4| z?jK@*r=h745W4u&+0ue-I4YfpTNh0@O3wyRGRPy|H8Qr`dMf-{xVW3K4!Yr+oJ1=` z@@sLmadW^zFcEb+?~3U42clgj}S%5Z59erB<; zO~r&O&fO;gbG)n_VOlL_P449zPw2IavgfAP6n_**XzO=0>$@QdJF`Q>$@mM7H~M3g zWDQhQU|>n(J%e*GN5l36x^~J0z4*qfGOWSYT5UK3uBg9!Dn6)`M(gm9RSW%aBhESo zN-Pe9AP);Q$7p4uY^R@Q%ao#`oeX9ETI=Qo+RsQzOM48IO1F0jGtlgv4c{z z0ip}`OYIU4JF*^$&I=QA#wa}$HI0zX@bOSdB4Tz~>{QBaUPY2Vp zKjmcmb`G~ovDqk&kciJtb!km1u53jCoX(e9Sp+%3_+i^qHgwN@86m^>V40 zpJ`$@wdsg(gf&}eg2>tf2|Lhd{53=^yZl)rPq-HHrDdF1o2$$1HCrX7pn5b@Kqxno z(1aynF8QdV$3O+h{DBBlbmr$TJB<$t-w>9!rjRf7*gvT90KWN5i2|R~e{{Qzt3Sm> zt2Z8e{Gd#7$XYz-xU{HMj10+F{}B)7vC>zfH33&sbh6N8YQKn7UfLXH21fM)HJRsi z0yR`bz|*DS&(K5(mr1g4b2`umF1t|g4_?!A*V4cIEsVrmSdt5-sAS? z3st%0Lw>C&ZQduqBdfO-!KqMooVH^cT=Zyr0v_95!~^bIrVy9Lylgc@m4a%QjK{$>Y;F>IP1m13EeR({euY3+XO} ze>;*P{|(Y2A7Q}-ru%ytol=SJh;I^P8+=}9_S^MQ6Je2%W&zV9Q&AwoRqHP_$v~g8 zSNhPQN)9~furjWc&nTgAeBKaJV}TGvrme+Lo&zy;jWS0xN6)IKlL9&XVqvdw<3@wFlFr?? zMor@0a4v3NX^+D;6sa4JMZe}HC`?B4?x*!Y4k@PLqMwW3DzJ25RZW^>Pfdak_K_&7 zsHqX}94V?0Lr0kQV|Y3$^%ZOaY4r>oW)10|E*+7Mvf~I=)Vw(;yygyEc_@>>%;SWw&Psr)_!*re9v%UR-%U}vRAz&cK(qqM_D=RDe zrQ6{hjX@|LlNRJbi2(|{eiElO6IlJ=7^Wq`I~8tc2?7G#-0vYj?AAx8^W+mLEnBV? zAL@lI=CXqylu#6!pIwM37(EFfJP-V9+1a@dKCnTib&#om_1;`QG@gEUop-bYp2>z9 z+kn6R-$h|?IgS-)N6yxQ&Dw*^XvE8@m8gzkuh>-`qP<{Oxt1&AKNHi)fz{PY{@QJt($ z(|An+PTclbq1n#Xy~R~DUNg<}%!yuW=ZiF;z<cJptq(;R4_vpr%c**kRUGAR;fodWHTTjq!S%bdKRK4QpIm33P)YSY2!Ra+ zh&`SRiC%8WT=lEpwn1d@K|%~q_l$n1I?%1GOZ)2sctKHSK1}up`{^`s9+&$CD)i>a zldz&~+kCx`xq3|L!w1=b1AlZh;1#=j-HO!uD6q6yJBFiD$pL(aL9Y;kH{viUg>^)W zsNB}-xGAyjQNOWcDgK2hs%?H)?KeRg@MMgVFo%bA;~!-%+R zJia}sd4?rA>#`9EC)n+QmTdQ6(F0ybhE`GI8H0gGwI1j?RLOI)BFvLSHwvQ4?P6-a zFedyj`6{>1dmh?NN*kP(d2F;DO#>SXCynMd6nbdank(jeMz%YtizjS-w8+M$gBpTA4J zOb!}xe0r!NrQ58{hY}ys7$!*sp*PqYWWgePlyIdtu}RH2=9&7}$8mE{8!<(4O_KC^ z$uBDB``qs6)|WlshLga?z2SA9<=qW(Uz2*E9v~N{@_Fdktq&DzzFl-VN-L$dEqSFX zsZ{TnY0V&DHruXVOmZEkg}(jPo8*Ixq1)r#-Pt&L~b^(9Dt#iHEO#?b|m z#QpJJYO$8CO8T4xN!hw}=! ze!0f;P+^khz5Q)*1liOI&5PP7mAJtY$c3KC4nj!ozQ^=S@ftmN8A>#WU%@9Bw)E zy(zUbgD!XCuLICGY;)^UaIml6P&L_K1}Ix_pP^;D?%U?S_B!dohU!2kG1bWKx)xG6 zWO55EE2J~TboR|h*LvT@q}6=-E&DQ-xm3_nGK?(sZE_yC4;f?)Xy8G9)UFBI)o`%0 z?Q`2ER|lOTpZrg#;Zqb2x5h@UvDo`7Q#fnCc2J1n>;Ow}=a_B%Ek zegJkiv0kXu1&L77X~`ial44y}ZVW_&;$Gw@QXMeAIesWCdt$lE;ik^k6HQDFgKzH7 zev{_usNu`FxJ^cfy4%E&<$ye^hu8$hd0}beN*^-me(#s(Ma9(oB8}(!zx%77^3CDC zREz9qZqeJd(d8%q)*fa@+j1I@QL^2a{SN$<@EmE}ALCs4^@q_QZ(JW22<*)wnXDI? z=(|!)JO0Czwu%OnrfU7KEy~aaa$!$BW4;|F{dze*$DJ3qSAw*d0>4BLkcRB2Bn7Arnz&W3 z&i+t=LnzH@0nyOg`jSkr9%Sk@XcoRkdxqfLmJFfRuEJN`r{fDIikP zUDA>wz3CK5=>{q3?ha{??(Xic^X&KizW+aG&I~ioILzLxwVrk76&Hq%Y-ZU=v#sWh zDaWa7=WW;mCZR5nw*JBlcfoa$R$~R{TG~7;E`uwKr(xRLh7EoADqST8o#maOVCFTQ z@AS0=+thuwQkwtzP~^j|BhvSs|F^LaM1>+aVTay0%sYu&Hk>JM&DQ*MGhQe+{mI;* zQMJ$PPxkgvf$EX(Xps)fBA>vKsC&)PAH}V?8~L?U1*yHV-@G@w9EdTUKS1I2EBBY7 z$&)~f;XuUmw7U z-8|dGbX0*6>Cz$&-*Ehx^;}b)z6B(f`0=&&E~NjD7nml#2uOU9K#!(pvBmrI(ehyA z>Hv*1{om$6pvnC)g|6hsiaBvYTHCgxxG3&07!*M z5ZQG61WtaO%F#dy6>v2u@BDu|;uC6Gd+sVAmYC(moE-P;G?fo;raT;VvVY!`Q>igf z)RH66*Ti()e#sX)NnA{SeG&7$yW3VR5Ldl&CvGG8>)HzDf4XQ-M-ha<2@d0d_Ky`q zS0N$x|J<@>N(6e+ALY{0(n`i}nW_Zog&iiYVX%c!RrPX77D0=BXBVxDS= z7xgMvZ2A}JjsBa-Zxq~aw?4@%8T!^k0fIm!aO!=_tuFMlHZ>He1u&L93Jh5-?VhgV zYF-!TB>7hT3thm0Tnu8p;Mrk7pNKbEc(?HIIpMo$$>E5sVbpO}lai^D`W`pF>@qgz zGh_vW!^Y0qwpY+`sx>w%H^(XexdN|Ew>tZRKAmDg*#o5`4&60;;)k+ld?#_}<8}6P z+zqnr&8&VmPkOiuy|gHl)bC@tZeWrf2dV%UmA_CdwEjE%oofkKj?IUEQS(PW&2Sww z?FQ%JbTN$I>J_>Bq!%g=p-LMUPw`dfL-+vSQ{Ar+Bo|vCwy}fd#B+s=MfxJZ*PxTK z^CIWFp<(B%SMM4tKSX0!YBg9ji{K6*5ShBPJ>im=uXkFbLc#U4Kkt4GS`8m$PjQMp z8jyv9Gcsmh06zOz?bQon8SarRX?gM+St(mvv9m_EhS#RH=6)`4D@RC+H4sgs1=7D2 zN@)N9kyo1gQZ2;wy29U?*7xjsV18VPAXh=O;`rrm`Q%%5 z16A8u;CDgg!=5VgIv`lwRZA4%)OA@+^8SrB&>P;*vtXi0CHSwRpdc^JVHWxuY+Dim zYxFwBVn=u6V^;u=x{s)-1pjEDgeL;8Lrh|JQB%2v&FOUuBTx#>zcEyQ@ZYxcr*F~% z%IP_9#!q3xxq8x*)gGV=&qTtNdq9V;;{P5raJy7Mkg1Lv%Jd$m#+^a%6&@~cb)yZR zNbZQ4b-gDf6whVJH=Nt4NISF(RDz$orpt`6TSDJMKoz%7PRhT_%WHi^Wu`pIOG)+6v3eMBkp`}2JZiO=A7@M-H|OsF9;f;kFl&R_W1F6%}0R4n)!U# zUFFx@Vg#u@Mx5pLi-c`Ik25`TtT z4_ATM5P9aV47AbI02I>|U?^YZ7(PiN{dKh%ml7Gd%ED6x@)A@Gg8o8M+5u2!$lh*M z<6VvKdG^ug4b{s$v8m9PtnD+si{n2<^{7Kh3r#ui%Z}FE6fz2gr@5oes8DY2t^;Qs z6|4RamuG*MEXPVS>(lw)+OGD!l(DO3`(pLy$Ak0H#r{HykUt6ELp3%D+5Vou{fj4~ ztjlnD{?M?zNb;&WEQk1K+&}zpc-OwY{(Pv*G=X@9wP{VmjBwgqq#VEWGh0Ub(x46b zTAWfl;1=MozS}w-O{aQ#BRkFgdGbFExZo-d!n*~aabE+jeHHjAMFQQQ?GN>L=QFn8 z;&(hw9I?p(3zGMj{$JfYfY`}x(4q1*m^hs0u~`-cbymoEd=dacCIaE`pJ4n$H8b){ z9DDC&6X3?R+;ET*1}D9>5r(6#lHBgD&)6+z-_~qpCBT(7ykxwsW4ZtciZ<+#7$(az9E8A7W2|J0_`Ylvepz&C zT?$WX-|whhdxA{l-U}fqueDL?#b(NKiOKdGUM*&3C%gV?v*_sDwAPqV+em8*UI3iQ z_E!sfcGVZVQ#5c7yk?hXv%gD!27iLvoh)0UBreF0@A&gQ!~;QJE8|Qi2!r)~&SGgY zsEOQ?mA{hxC!GIt2n3hg6kzqvC>=M4z@IIn8wTB&(EJ*FUhkiL>xxf-{Mn5X`6?c8 zA=P?!ekcm<>-|mNpKqu;_p$m-YAFYoCr2K1s_zt;AYX;0`VAo;_8k-@nkW~j5#&O5NtUM z5-LOb284@aglJGqsRf3-ji!Bj@WQRQimL3CsTVTRu&RW-Ms#t&n6z-gaq9<1#@%__ zOeWws5yz(#dUXu{Pl4k(DVz!p+w@5z$l%|JNHV%zyA}ltTkAKllxZX%RnVunellOz zIHBON{}`&@!QCaVtC%a$`%v@?Bf}cn#ZJ^_;tfJ~cpo;h=sFz|jxZST_~!fYzb6?A zKuFed^fMLW+BxJ976BLOh!a2$)Ll=W$4HNh>N?8nZNY;!1|p=%7zXevznnFudT93%CK$E9+K+MkdCpGT@bf)ng z9$8^so(`^XF>HH2ATZE{xTnG%E5itV@`Zklg9$v*0nq2sUPj-c3}_9;E&$>|`QEs) zOcGxvhMfb!bTJh_k}{2iL)0s!MDJEoG;a?y4~*Ti5deGn08 zDpp2EIKh5Ay>Lro7>|09>05Q1HBNC?wIBjZ@eS6#fxo6EDUcLtM}fab@1Oc9x&eKyFi>LN|xQRq)EGK6MZzzvar zDnIIHU#KJHIYfOL0{y zg=?*e$(uHRf3dNGgJ^vo*4=sVpcr~7;P{s+00W+yTu|XUNIS9#{yq(w)JMw}-~A1z zyIvD>eMy|l>agSPvL_1Fx%jTHp^To37KeMjS(oxSurt>4ecbeS5`#>%o7Ds*{qesJm{@btz`9T|03kSX^3UB5<# z_4=jDJh1=zS!L&0nQ#>#$V-i&lmtvyCBa014h`GjBxoE%h}Wx&8_!b^8sNWJ<*4@v zk%*;*eUBVv+j>Uq`xY)5q8;U*1jQ%&0!PXlqTI%l1pf|9BrqWZ1f$<;OIHruKOb+o zf1XNt*t|-4AgI}`f0&-!_d21a+j^j-yLq^)8t&B`rW)ivp&TjjsipD|QZ$%<<>YC| z;At^jdoWDmv>|g$m6BbaJYI9qR*SjeFWA4;n16FLO1^Wh+fy0c=JfAZ23SEB!?7PZ zq-4yUj9~`eUh?PU!#?9QOA%@dCpBtzh#YD_P5(~VPgi!4BO1GKXkQ(%v$v}4#Kxln z_lXS`UZ>TS`+ytS^dG3lu8P&s|CxsfZbDgo8nSVP<|p{hW1jB|O|lFmFe$K$oMQ7| zRqeh#3gwMj;T2NPULT-)U0W8;yc}xa-W9k)zuI2%)%nU!2!4^By{IIk_-x$EnTHER|aPQ=a)Tz;S2 z%i>)2&T)@5y{J-v-dL^nWJ%sJxH-sd zdKXjhIu@IA$?}~=nkWCS{*|CLD&_qTVzlhK;UD`l)*b8)j5NAfj*Js*VYOoBifE|p zR@Wi((>T4_SDPjtDqqsWO_fWbxxGHkbj`sNAh*Q6-WBMMVgf8kn1Zx^Mv_g!{ zH`O=gSN2};>Pi7_2EsU3eyp$KmuN8zwm#@;wFV;;nyNB>)7D$XE)*w1b!}e@$h_YA ze)L}x9nZJ#o1s_sNK@D5&l^_+SAX^C(mCAj1Y&`eGs%}}jKlV7Yj-5_h5-uc1E`SuSz16_y6A8x#C{6JW9x+^QiVer%DqzrGGS zT-32Yr?~rsS_G@NTNY!R|Jb{mqnj&(Nvo0%G{{wVbgjK`kj~S8LOLIPruyNN=I8lP z%8Io47k%SfunRG*EvuNEY1ldIg?ae=h1w_h(*Rt3zxP|*dC9nqwQgFB)&b<(bm;4DNPY*N7-tS z^u_x%3g+iDD)bUeCoJe?&O>u4W<1(Q6Hx?I$c&INH<{45$TQ3=$s2bbn*ML9K*tI8 zBz_0F<;aBe_`l-_&n{}#t5X3zvNO!0TS27d_XLmk|7oJ2I;iz;%_3L(V`Q%{4k(8I zOKaN8>BH}h>78UV_&BNWNSemK2?OIrV^3@A2%D0Qsr|kaWuOuZUg5 z8{&U5p07p&SGL|CtI#0K7QF{vcNt(i=q7&XvB_a1(emg9yK8(;gU;oXea&uZucoJW zzx~QmkSis!o^m?0S9#cN=Aq1i+(RCJhC~1jL*gK3vn&-EO%#GeAF!8INVs;I$ZG7?#{Uw zuMgbr8=cQv4J2M&8UNO9sC8RXeR~ex`kwu{x^J-7?m=GKMMb3h`r{)mWuU_L)BH~IYB5`ENPH3;f z*Ic(iZq4?F%%MxoIBBh_=I^H%2EOM!JQ_VQEG*IqoIe$0-gW#m2Fh2ZcHOw|B%QjA z4GjZ(*CK%7q6q)>CXHIuPntDFa@0BKf>qs}eUjC9Uz{!Axi1tb?!( z2lu;NXu+F6z{l%l0wU{k$9uT*ftSjlN8DABaav`N zg<+B}t|sig!e-7-=1-Ldn+}Dj3YR#N@Bb792 zUfI@``J-C(W%yT@x-Yo3gf4?w>h6+m>vHUAOJh@7)(=A?qYr-Z{6=5ENQK?`)Pxvl zyG^ZKkm+?ymz(}_x4V13Duf$AgS3gqYW~>U>`&gji*nGMAaQ(b7d0$Z%4BcGhUWUf z{XygY;NgDI>di>i!q-7Qw-GVhVz6Oi2jb9oy3m|Nxf{XAD)R~EE#TI)fqVz*M zCn>-nQj0Hjs8EY%&`LY?WQGj8qNq~JN~N#~wmWk-w8Vx>=_yL%zfuZ()fJt}gDWefJwI6KJYTgwj~ha%?X`D&q0*?r8=V+N(@+c!+2#YGPs z5yRV-HhF(hOD*!M#h>6^v_5uz&N(1Pg9!h0y2IjHmd52ChKjNiU`3eSaatDXwlqlT z)^s~8rLG|;5f>V%I)60p@+~@Tke6m8OD5{!W}@kH{O}ik<<93H<7t6sSh&`!J>bq) z@Yw2$NH*K}UAVa2CK=}Q#e2u4jg;QY7@cu{6zpfTBiYg;hWzt&Rq!DRyY8YO?8}4a zqgu_3n%m>v%=8YPhZ1p$u?z6~Hoo=THtCG2#e(l&-a-6M4wPi10b6N&X3|)6MZ`>( zs8b$$^yml2K(n3&p0<;ia*mMibF(l6J!frFE}2I8N9e5PL>C|fm}L>|ri#5ssO6hn zzTRZzSr-;lX8hX8gXlBn>CY=X-)!Dk{e+so-Sy*2yPZt7?_694w>E0CBdl_h<7)lJ z3W?gQ8LZF|xwZBK{~;9_%yDD}p{aVwW!+IhaXA9ka# z-;W)!pJ)E!x|t+y22c3`&ZAoeu6QFasqQmU!kK~C`)~@|@hvbHy&^LdL%ILs!{y43 zD(5TgPi5^i1DBH3&Tml%T1scHOMr927DcV@2^$$&5VZ)}Ch!uu22VOLJIfti*2F}DmX9>Tu_vayI>9WU{AQ*g_Day}0qMX8m% z$KyLIxh?tC&SzPk@p+EvWkd2`ef|MYUu#oZK_uF+E(r8!F_1n5vo;V8$R0NUY}tEZ zr%I~@<~Bd9sG`V8tb_Y~z=oYNHMJE)+Xoo12;Pj>_-`d!gUDy*$0gr|&p$Ljx>kL( z<|!a^HV3s`st}Owr!;XKcxPL8ygFz$HB7TE(%fl&S!TvM@kgCuE=TL;+_smgN}>~s zjQ6GW{ng3(){Xl?ZKD0ODFBtZ>|zd)Br7aA9bzhpJI%GRp@*a>?~IjYpDMCl*rdn= z{Xj@O>X(91po?nkrqXdC&ZaKCFQD~t6(Qg74+|A~oK5_mZ>a=TN}dD#Rb)|W6_{z} z@f>mt@+xrW$lxTgV^Z_*c2GSPvPS!SHB_$|Zs>B-<~ z_s=z%USWDT6UsGBKeuHZ?0b|(vBTn+IUNsm5&aXa8r8rabvt|1O9b@kQSZ&riR*=c zS_sdW z&_Y=te?xyHy;Z*En;9H&h^-Z=?_3bL{&CNDv*XFpN9vW$-!Fh@z7XO&^=CUN?V&$a5!h}^YV;si6_ z`ApITAxegSi51IYv%91RB*Ndr-h;a-yvy&~<6uACA|C){5c8{!`WadoxaW17fw@8TrZUlzQhnL)FT zcW1*Nw`kyY9%8R{Wmu7eA;{T1xgV^qWVVPL{X9nGYikeuk_wXXZNGl4KrM2BTxQ#> z{Ne7{`Y>`0n30b%u=a~z;tk3@$jKznubl&SD;C4SJ-;k3j;80wwHO3Y*9_y>7whTM zUG;Z(qe&rf89|S6l*L(OG+LQDp1g#>M!O}sf}tBY1M9%Mg|I5#$gF+?XB}Dp98|}v z&SUbS@-#nLcbiTg_9IF5tA*U}ZB|0nn>$e)huRG6|44E0E^AEHmzG||^dEynD#1`; z((V3mqmyKn8z1kBqlwB(bd(6J(&w>DNJ^%Pu~`f*!yUytUbJ1MCjR}y<82|jx2hLlptTyfks z>(vtY5Ig$w>uoL8IUF$m8uem*aYD?=*H5$^Y^rJUtE7|Mwq{-pQ{A@7l7=sggdnPIh?o&uv4t{Ssd4mHI+|=X16T93g6|! z>TsszS=?bRENfcB+D?_Jvgof_zw@7hL1>`lLWuMA_9L6Vn5nb9@$!HJ)A|$G+1TOR z_^hleeO5@xg;o90ds&>c)3L74G4m5ndu}Vd^vWc=GX<}!r9rscguEezL97SkU;X}> zC26*hEhfWHr(33AHjeACPr;p};k)HjGv zNs;K29rY!GYrru!%pB%K&i(r)#qBuoRF2A4H^5UzyCczE#Pl6UBp07{UX*rZT6v^y z-`gv}^0|Umf(b>~YaiHb8wnHS8IJJEsN4=3vO%UnF>`EF#h>ETeZmjkh>y(s9(1>R zUY|pL;PJn{RSfXB6pl?gSF7dqS|swfHgd`?zE zcw{gpcOu36CIs9He7ORwiowpvRMt7KlabeCkJo5vC?pK|@V$RE;*IFti*l18VU}nw z2Uo1msus6Ey?tUN-&WD}V}NJU%r~sag03UrnA*Sc%?eB3`6)8Xn=g~8e5Y){ydLHD zvV{k4i7JdxKa@p;hw?s^_v2aXDATDps*dO6mrFgL79LL0x=$T--b4_p2d0Wej7h!2 zVWQSYPNAd;2Cnp;s(6C#$EDw40lqls0$IjDnjP<|io33iZKxSKOt57}NBU*ei zv3qSFc?MpsUQ;vfJ&83vA2q{Ei<2=G17(87ga)O08O-HnF8cWOo%*-_XIujbk8aLQ zv>!){A;H7rtOC8%d_hmk`#|wbel_ZsRTZZ?-x#xE9qeck^c!|28^m(a>lDm}E+Re^ zHCEqTI}9AT(2;!8eW7dwhG3cF`OJvGAd@D+dcWeZS9n_VMd!K53#?Z<< zo%c14RzfR14mDAOm08&HNEvM0gE|bs=vF_D$Dwe*`vPtk>c3sWMC+53gOR9^)+X8p zqNvgy(OFZ~!nirZ`(GcUsgj{QmzE}v36WyZsze04#hQJ_ZM?)AxvsoAY!o^eyLMpp zwEUponVLRvTTJyVH9R5EdK}OJEved@dTw5oKSq7(k^HE<6E=spv;Df|`xjLHb4fs}@Rs9-D(NnIoUU^M z%ho~Ed?isXL$w*-a4|axBDME*zi?!qY_xwwN4+{Ezl%|K{9~pXTOD0R7{Q5HB}wTA zfrgkTf0ssW!4-aSUrDukK|Cuy(90_;!?&mS(jg;pU;NEoy=46SsG^%9m&@h6v&Q-I z7w0t>5wxSStr@rj9*SDn-09xt2-Mj)?6?|j;v)rleGYa%@rK zGi+v15;xr6wFx&9Ap~SyakC8Q-36blH#hRdt9TlQBcQ!aYW{s~dT+wyu1<5+KoNdh zSwNe)*Wi}7uZ+d@vZog>+H1p)tdrFGSR6CF{?8d*(~bA?fZEc_?7uasGJo3u%xO1*eDa7fY=8LmBkj@z2J$S3G=4tb*|SjU^rCDAqRYUKVlK;f5qW!ma*(WsaF`?B<*)k$i3ZTp%CWGe z4WK7D$S4X8eXq7+rCIgcQQ;l59ebZHD=F(8`c=VNDjD=v?=aOr){la(UHd)Rk=JBM z?2xB2|HT+Hqw){O+v2aI*vVsyQP|if&(I>DBIcjzX4z6d9rq!|Yln3)tzk#UP&8c+ zeZ}PBU-=U}Qf6;G0(USFR?;s1ETqa|1whs96!|b7WW0V(p%mnn>FI3ZkDD_wEIO9_ z$gZLG3;Qr(9ayRZ_sNvXf@O%t0?0T-t{u%gApPb~unS342@-tZgC%kOeK%Q!LfyUs zPDG%nU}vwmw~T(l}{YTDa#4WhmyM+T}Ugd5Y% z)mLsWj-X(m!HYMUZuG(5Y{aR8gkK7^*1l(j3fVGgVOv2_(W|{~pQr?ah1#x8Ouo36VIQ;U1n(?}lVax#YFUPfE2{fi=>u&rjK~dT%H0XnxT(Fs9nRYrOZ& z@xkw6TQ=(P2pa{`z)l*{@?MnpfZn;Y3)C*hF(_Nx2-(g{-X4H(xA^i|(608kZl&Yx zlOpHyB@{#f1gPWy--V_Su_xwy90mbnSOQ~E3#BC=?{}BnsP16BVnmu+)X+yolDEws zF>D#WgaQP`FyKCLx(IP=W;H<^>}iDE#Ak5J@K5{LE+z$r>(&>OBaS~6RDJC8!U;`HLlo z6aX_T>z-DQIgi5ayu&1jPMb7*=KddD*=y6M;{QDs|7ehgztwZ$;t{fO!z`PJ>73tD8{vBWvG$3+e3yi z6e5k|NZ4|Z<;}X)$aB26HrBP_)F=@Y-6Y4a?T6D(BQFZOp3bofMM|4p`lsbeR@FN8bFJ zs`9s;7c>>wo7uZIbKEenh_t5ogfxcJxi_%Gp!53L9@g|!2$e?X8-|g9aWAq+peg7y zFcD8xQWRfhcHGLo%Db6D$H$ow`n-YlllIW1f0}v*V;u1$=JLm!IAr7P*Qz^pm#hO( z0i!-1%=oE62Y9VO2|c)A0?L>_?eLfXnEE-M02rXLqA)g3fkhVoI|gMqj;K$((O(cD z&6$m%4Zl!=ptUA;R1dPK=J`89S#MTPiSZMeW0GK>c!Lx>BedNvFEv^FAlA6xTazh* zkE=O&3=4HhVWFq?Yco-gWVPG>?k|)z%`Jspw9GWde1GhZBRHFb!hV~h;d>&u)xZ~A zu+@;At`_=u+D~qw!Nq}$VWEy6>c2Q8OCq@)39y{3b=pm3FjV=6Z$ z5;-vwJkF!vKKs*5Lgy)=j$xB(7>xASHt)4&L78tG3-1n!9iL!>UxRvg9sV=+`!A2r)Jlu6wN z%QgYJuiSxxlFn8By+RXL9+A}!jZN1svh)^nH5IotQjbSjkiDC^98YzLIUGN-B?f0x zdS24#!kX=b_t=7v$*+{#sQ)@n#U1VXiSr)|*NeNM-oyrbUtT>KerBRaaZtXAbGSeA zONCtuJ;r0?KDo^Tt$FC2M9}1o z|E3BhzqtTvxn&k}UE1+X#M12fb^5h@ZpV&WcCy*A$=<#{5zs{6BVdTiCcc$tA-KC$ zcpRj*;_z0{+j^nMMc}u5fuGx$Kv^R*wM!uG*+yTa4NvFRA)g9o&8yLLJbu1?huwWA zue}*Oawy3^fsq#K0C}$X5#RCe3ZMFtH8dW7C;#r#nJVdR^`ka)@NHop*hqgECcsB9fVjGhHtNd|;V2LRGtYJ9_&M1Ro4W5A&qw#AQD_igkBhf6^J@ zHE?uhLJ&GEafVzUPbLDJpTZBlMob23bXw;?5;;?>j36S}p3%>}PqqgyOfV70x>fNn zKwC?~x#6uoXtvX)*}h`XN4|OEg%L6quGsT>Sp&OHWpo?aq@Jmvo??!tBj~7$Ttl@+ zVfVq9rB?JJ#?XMVg|aK=NKpoZv8)Iaj6q4z&A1?~UkxOpE%PLwK_gy&5~47qs{eiQ z%)BwV>Ei-%A4kM*mIW|rSxyj4pRFnf(Xr3b^u4PN<~YA8DrFzmIR6O7B6WMn(O=nT zNRrck0YxVCDUT8U;yOF|mz$8iPXYza8gHV{GQ%jOY8ewC=~4&+_0VG;`(2{S)Q78x z!L!Arrg))h|4D3lie&+#{aD!jFc9K zl2G<~-&&b-s)3&ZsG~%Bz2=;2(O+p#5{|L z;TLs%E8~l&-nktWs;t%)aUI`HdN*Ds3jggx+2;1{sx0+Nlei57Wo6fZ8&0!PD6h(J zgg-*L3vb(UCf^B;m4EalWHE{&#NT(MOLzN47L0pp@d<`^7y*NK_MTFVHhy^yVV-lb zCiwEdXg&;^Z46D_SJHs1jam%>+)0YXdwy*f4nUu>bYHY(5 z-KG135pgVojH5DEX-$8H-2S<4{pY&NYc44S!tQN;16W$Fn}7AX`{$UUC#KzE2!n;H z2!!F^%421VZdb37eCDRVM~PyQDHe&e;LE3_cQrP&WIkbw8sR%xib`=)-gSdOLz|3( z<2!1bEdyc#y1|1-f}$$@bqDRl=c9-Y_QN61+ST&%*Kxr1DI?o|d_r*YV5Xr6tHrze z+EI@{zV!Qb^~-d45Q4`cUR*&;7ahN|;pu25RC#n+Sit$tMUA2NaUl1Z34|Q=0eZ01 zLJ=5Xb;l%jYr12sbG8C<3nK1|mq_;WS9D4-as%c*x4q(dUx$Dt zt;FrclO>tv9G9$&5F!nP93u;xP$%U%{QFlgf9VY0d>W7;yx1mf*xMfynCueo*2=UR_7S*X*v!f@N~fkFywrPa_j_AM|Z{R6BabW1qCITvS~e zdM$naDnY4-24UEA2bM#}*SiyI&4rKP7Mp-w9D2GvCRJFo{!FdWwFVge(SLpqJ(Jdi zeZQt!F+l$3R3hS9V(MbqnO_SraRH~hmYtk9_fE3TGZif8{h#94Gf$+hSx2Iv1>cwnau=ACHxsh^p%N_I;0qftJu4 z%-b*Xt4#q6?Cp2mD349~-E>xue^77Doy>N}~F4_V3Kd(SZJ12vz{RB+sA z2@u`A?!h9+uuYk>ZmU?meW0{!sA58!0bl`taxJ)WykgBy=STO+35^r{XVVOSc|b0o z1VzDUq5KRRzNnTQ{lAwYXotpHE@S8PN5k~*0oD4B3P2FA0aHDw`EHV*W7}-&C*zvpsOHmcTJd4S} z>Jk~Xn##{q29anh@f8%>S z{OvFFzdg>Mgc7rj~1 z37c_No3w{FroTIt`LSx^1Pa1$=R+lWW;p29N*b5lIvscotDeb!lt=n`Q}ra`Q$=oo zdj6Q|eE`)mIoGr+<_(!ei z1pVsPdUdgXjnd$+3;2OP^GxJ?7op90xYN==H7VbWlpnwt6q_?V`^IxNw) zr^MtbT(}kJCiisHr_idtCBz852eeAU%4HggOO$ zSbIh7{^bbUXp1^LA)YWp{y_C9*R*}Ex5%Ej`H==_I{fxP8ECR%-~|3)EIN<3mxq0W zT`yG4A+TmcLd`7mI;#c#-zr7Pm?UpL0tp$~%rV9s07}|47|T-$bqX3D89u{IyEZ*D zyj?wV4%c*^?;z!o{r8RtBH4%ux~%8P<^A*eT5}_!0x;rnfPL6C;PX^;*8k=ZUb5St zRocn4YJ4XRwp@s?&8~17Gv!m}tDA$SwPw$zv-1@9{Wnv$jX)n*W3ZI}q>m#5pq|;S z7ZIAaoWdaWXo5|hsS)PDwFDs5)dM1<4~VQrwASx7LO(4OpivzdJW3hea)XcbEEt@QBQ!^htLGX0z8l&( zBuxO+a5rC;z5O%v*}2dt3l`f#BA&TdMoLs6VW*hDV;c&#^M5kKKUdu$tGhNGHC&^$ zu`Iwg_)kG>BeQsSArMw~(5SKB^w22GBy{WCUih${P_&v~HV^@4;i*qgeW(|pgs*?? zcFx0Y?{5wOEZDF?>qS`#cGaAH!mB+0NG6xW#dla#3Z}GBMo(V+KePD`3EZ};5@bg7 zlYJivn;`6(FKzd9(pcUmER>b4a`i^u-rUn&NprN{&6QMZr-nNSJIUv+CAtXw=x#!Mp)l=W~5!VXR_C||C3TF zJymfQ8EaI-4Qs$L3zS)$cj4*j3jej5o5%k7@(?FkZ7@%{U|9NKc#W#&{+eWMJs|-{HY~+5cI5E$ zi7B9U!TzJA2?Rs$-?yu?8x z){RZ~*LiPcyT5`g=p(rZa&$D11?Wi?db9>USc%F3Jzb6x+07Sy&2W2^>+^aa@pXk! zzgc%5BdYJS^)0)~Ugo-whu24)2WtgY_t^tFtkzJ;xV{8Z$j1QJzCKwpKId{?pkDqX zAq6r)9`tAVF&nS;UWNCSdRM3k+XB%W{$a0U6 zVd+{VsFfLQ6+1c#)`Nk22B2#kgBZW#uko`mV<9-ZQE61%#RQ%s{`ncA4=Z~vKD-lk zd#(QSx%BYzn|-PKn+0GuNay;PJzxXc>dl!?dh#4R`~d2}HDKI!ssafG2QHG;Q@epY>g&Ke zS$6iqXf}i7nE3{b*7-;#4dr=9gYUp~R}jyw{sMp2NTnG4kCGDY?Q7ENOVwqL_$_BXW_OdeIP&vuHcz=PXK?J-PWr9_~j zxNgvf5Ryy^Nns(O%5rQ9-hRI#8D;FQ#813}`Rd|Z*1Mu#bst@HDo1!wAyB!4p@kXu z2E9?t23g6m^yITanF>#mKfnyIXL~H~%zNu)cA`GtZjJ@b_Agmcm&0v4S$gKpy_~%E zp8xb_Jcx{L31hr@?FpW89@Vm*nK^NboxeNC^gswv!jPJ0(Se;seV2qq3Ho%Z3#wY< zI>B+Wa5!MO<(xYAw^yqE;>M>Z*uvQQ_SC$1IO7{}#>wG;dGI@SoJ%T&A9m^$aTowM z1vi|jv`|kK?-C~Bypdsnk!!fR>1B+B0pybK{0!Bkx1= zW&`%0joq*h;0Wy96tEbwXg=xkqu*$b; zT~*`6>HGxUzS;Z`SUy8U*_+OIpPBzTt$MftpOvj+v&;LDZTJ}&q{!WGZQi*a7Jr#3 zky24?5qU?AkPt@+;gXG^|W1 zUsWF4!!I$M7}w9^if=@}F|fuPyL^?B6`+IcCROS&5XHP1jh>z|Y|I{*zCAhy>(jmY znEBkHORr)~+<-?&ByYYz86C|x!m-0iyuAYWQcRHLDPUyCyaGDcH3Vl=f`28Ne?wTm z{#mod@5R{or?VE!fh3k+TGZL^l~3@rAJ14eNS`0f6eT=bx~*Ob8PVQP;IfJa)h5fR zk@N6BF3KPoba!`$2g*LceUr<7ynzF+T-Is) znyO%c6UhCPTK{eYo`RYant?;}P6NZ@2KP6aNH77D|BEg0$_7ucN<6?d;F|ehXuI9w zjfN3wGY`d*{o>YqW&T&N-PWn+fh}o_vOOIn%JB6!3o>E~ivk6G7%fDGXYaMF>!dsX zrEJ{!@@v`NrJDI7ae=`N(b=y0*9wPTe5b1-5pK%0pEcv<(;vlaa!BZ4-TyOVqJ?fp?3mYX7ymDC|P1(Hex3W{1;Yp}_`*4;{5MkhQeT|*`YO)1o8n^lxo!tk= zYMmQUCI(y`_B>>iel^(j!DXQk<`?Bbz1lKOlP!9cR)i@l13ve^k7Jp-X-Ke1( zQ(9#8yF7=>^TYKTD7-fsOF-^di-vDS{mw!hF9I~&fne#@qwyAeaZ2=HR^fOq19L(z^I)dsh+Tjdml>agmuVzOQSx_Q&xc$oFsr^7bV)e<;tL z3-<^HZS>n8+5Dn+0D*~iZ$4E|Cw8YQY&?YsP?IAuV%r^Cd7At5@KK%|nh!g}+tE@H zX#v+u>woM(08_Hm{lV?`LPle9h3g>S1?ut;%`DdSVA%k-6tH^D4jSz1iv%Q3q0Zrl z4S|Q3fcb`bR_+{@YCchT3`_~lSsWQnFi1c#qpf-YAJKtNlmz~;XEnD;skUo9wsYSm zwjSGnPuA4(YdW>>gnB;!L8mBC$ZWR-bOeX-z}pTS-U}j;2SekUnY{~|f>Jr?Nwywq{_xj5 zPL)WG*8jUMah8sq4L#A52^LF%O%|Y#?$#e`@q})^eBR-N&nfla^r+5@|uS*1fMHL@C@y&x@qHDuGiM=dYaQLMs>{f z=&XVQzd&h3+x#f1Q{~1rlZB0lmkkXfiJ*79(VsvcJC!x2#ak`cg&m>oMQ0AGr=T@x zKaTi<cjicYe8XiRLiforj_M*_#&JwDRdxUk&g063%nZp5Sv4GH6)B7Z-xmxE_|+cjPQ< zldFuf9iou^)i+djPwJ0(_LbPSq<9#f^R@2WeyZ5~f<%9~J8>{c9p$;aHW<$(1@+6e zx@eTi;oboS-y#AMG`9_wV=H)Op#7NlZM-K|acevhH|?NdoEwS^O+jphRcJZmdZ;WC z)Q^Bsd-3YN!dCf=5n&}aN<@%jQbfNVhOQa&vxk4X=VSv&Ks;9=ov;%6>Lf zq!s$m7|{H`TqSwo3If4jNIv$G=yD1|>mHFOek;Gvy((k$&RiAtv)5e;Y=0y#QYE^c zoZJcjAF93rs>-eFnm7`IaA=ecNhwk3Zcsp^y98;F?hd6@N=fN1>F$sQ>F(~%e*=2o z?;qm~hu5L!^6Y2twbxp6t~o~^;v0H>K)v(__f!L<&*JPL+K(UIkRh6{kfaSIwrU7< z>*LRwW3qL0PD@JibULIx2i{JVNbh$a7A!K!8tgFx+~KBAy7y+tO98^-4TI1>o3w*m z>`gS>e|Y)_|R(vBC*j~4zcSogxBDKrA|2zS_D-o;<{vag@j*DBhTd+iyAMR7-lyF@CYbbqMFkFvnamz4 zgTdy}QbUs$Kb~pM+f=dlEbq5&oTML4-;YxBEQ>V5>K9G#9{70k2e}ZD&%fz4H^2QN z4=N8{FO+G&llLAO`Au6ibKq4d&-*H0ggb2fsGWL7^I>bSDVx@s;}c9ReLzwW{@|&r zpF`*lW-S|%z3&*wUAra#4-V{Iz0#MEGx07?YXUT_Q^Fj9E7oJU6pi6Ss)D$P> zgzR>GUghL8;8ko&RdTbbrxK)n;N{lvvwXLezq(mD@ch9$4ip4*@|wM8Jto>mS-#93 z1SSi#db&J#hOQ2bU{jxxdboMb>nx5ubS;=eN-JLO89-SUCErBE6erJrDMiBwhrj3< z5W0g7@hlZS@(z3CnGN`tc#NpxcYawD}6CAa5tC?eL4n z%~-{~Um0a_4{p(I@Zm4Q1d;uUn6vh`&u~&=LXH;Vg`H5n1&CAr)t@3VEOZz2>Pcmv ziCw5hZfXWVph%WN!Faq)dfx)UAK1=~Nz`{hpaV@vebRY`)qlFzb3?C3={A#PM717; z15t=W5lu}J_ivKd_--aT=l1r_4W)afZEo3?GWpKXq}#4HChVareyp(8idmuZ4Jv@l zT)u5Bjtx#fJB|6fq?qCSnR2NQb(e+u#WIK})~R*%#?p3?@R?k*GtikrOW!wYQR1-> zVzeR_?!DzTf4D~+x=iE;3+Ejd0DS7?jt`9ih9c~N$8DdtKKWo4^TEI^AttT_{r_f1ralp=ooO7V~?!It4 z!s{b{6%oo5U#P$>O@|l`8c|qJmFs|vFXN0vDoQCjV~Qh$|Ae7*1hT0Ea$a$Z;~yrv zZcKY)N;IEFhoe*P;AK{1eWsq_xyEV}M8iyFKhc(AHFlJFr;)D-x3K8lr;!?zrPF6l zuXyZY`4J z{%BbvWHS#`Lgh1PiY)SNL$_YVukBvq1(6xJ`AXt=Np!yWuPhUQde~XmIO8>Lk~W8o zVFkpFr~w;5UOW- z28JX;9+9%}rN=6)kP0^Mm!luec5M`Znt}x1C{XE8?bPws@PNw)uI>Q82yqTB zjmd25!_q1xJS&Sy(#&@Tct~dvbW(0VaLGqUZXQb**_M^tZ_B^SD*fy{#B9Z+o+z|A ztNtGpt#1OE2%Azg-$F0%Sab+J$%BIndS_>ggCdKAF)Yi9nj#Z;)-0fDF4 z&ddYrE>26+ff&8h*SGUz0PB1V=YY%X`l|$CH&QGhZs^8@dfVt-GtZUn+ju8(>LHA= zYKh6Hg!HdbR%m)^dm3qPQh9}hrdp}xc!n8kunW+1_;gkx72#?He_FI07i__ADUqpad7rYC3 zDMR?UzPU?xRotqDFj2q-^TC+I`489Og*5I19_9p|9Wg}Mu#wFJ=|H{Pm79SuKO$z? ze^YHYL+eO6jTG%RJT!5be`?CJEM zdN`wKo#1fhtepi5^E%}xdO|lN!gfaT+My>4Qz&0tE^FJZneZgG<4;0T8jH0)A^s$U z!7zTQJW*%J*K(cXm3W4$oO( z4r`ftxWh+Anh-+zZ)Z=m`!@gr`rNL)KXd!jq%vYzH(d@v!(?Ueq2mX^q${xtp3Sn~ zD^oL1`U%%^+j1t@4VvwCY-;x@i2mJHsIVIr5o7-LSo1?XCRr)c->J6QsHLtcpVnK9 ze|V#)&n3dKySQ?lbW)|z-aDlFpxB8A`S-W*Z?A}2YHX5l_jxYv8OgV2PL>=JLdZeR z7$v37&ZT~6)FA|8^&{8IZ2wm*LkWfU%AQ43k8{G2XQTA4S0Zw8TJeSH{y5PAZd1-r zya8vDlZxKwz$!Y&KnWqlIROP^(k(1ckUe0k_x5;nKE&eMpnfCU|ei!SgpPNTw6<-xMz+tF0`3MB;Vu`o`qkw5#ye4 z73WAiw1{HN6+TbqglXKa99l_^%xc^gzd*EcAhMqk>o)JL_cZiR2zGT=h>nY^aFg2w zMA^UhWdQ_gfOw1j?s)y+)!o)k2d);QjnRzPNc`X00bMO%33bQlGxnbSqc~Z;jg!_& zjY(TpR+KKs+@A?!D;B$*bk{M54rLF04C*vgt4ayx`WB0&7HWEPthD!Mr??(<9!(p* z>D~JB)Pom?HKCD+lzwbnY-bY*9){JOX}C^ell3AT!QF7UclpBX+ONy-AcPw_{WXq- zz3)DbfAQxBQpmt8HKq$p5wtM9XnS6Db+mhX%pI0u|LRCEbufL0me<6aKZ zpn7ZUz&XJ_|1#cS$b2}zM4*!**}(1rF;an)9u9kWuX%j~S3-&U21|C|r=n_K*-h^l zn`wAN1HOUNPH_BYfc{5n@Wq_L0k(<)xxs#;uX>J3juV&>LbBh%ZFPjxPnduLcnn*niRyy%fgoWHQbJGq5x;ob{-m)G7skmp^R2yo`?%p_9j^RZ|4S)E)gV_JU`P7q862zzl|a5Jos zcY+`oUNna-=i~75s_q+8Cw07R@lfwp=jqK*Q66Q4|2D=RR{c&*9jt@HGAJAwGe&D` z$Q^ujami+sUSa9BVGXn?3Enk#?v6yjW7yRb#;ELiRuV6;`}tvhhMjbiS04wjBD^&M zWje94=r>lb>K5MYFo;zLEnMt9`|WcSe}T7Wngz=|ud&fNnW-KbyLFdZm*1?#l>cEN zGew$MvME=?bd~Fx(b~DZG7OV)NPh;w@4q?Z{Z)&`8lrG*%v>l)TXCGvy~VM^q*2~B z8!?j~YnwV%|8xuFo*(VA!ubA|7z7|ME-g9!TrZdX{;3Mfs^b|c8DM+W6IgWyeB7n; zg+YWx6(lf!O}k6IsN@$+?iqj_?J01uozHKgr3Z1};I#mH#kpd10lnY3r2J_Ql-PFk z()gNDX&go`ur_nOlctw;S^rzcc`UHY+n^rywS>$GKQGYoU7~r#Yyu7b2MG&c(kMJc z83`cdby4OJTUW00-^M5saNmgb|eQisgETauGL?$sT(55J=%3_F4-}T-=kp z+Nf`dii3i*i9_m1f(&_k;OB@9nw?FP`BMCO#Q(-Xus>aHk6PPz3&9rot+>#W557$e zKg)=YmpYgfO3)LJY+%bCv+|LWQ~gWM6Op5#XQ2CtI|4l2s6li1kJUj1l6fC$46`JJ zbXI7yF_;c_)|W=?EuW;N3Mwd^FyNs5-Sy<>#0!rtsCNz3Bj?y+gK68GFe#>m%Grv; zUUPmvQIwnfFM#ff!JJ?-XIp=(JB02ACx-eOQ{WtuEmBlVvkTT{Qm=>DK_(EshVJ|q zEqsvy+ghK9$Xtx*W9Va6QDUdnq{htCPY_9PDH>pWM)cyfIG3D-+x-Uq$#;*nK+@w4 zSdZV&QH~^{74#or83?;93ONx5z6e5Rdhp-d*~erqM!w?UYpP=P0j5`W*!Kn`Fs*NnQV?2KGA#6ofG8NmXKgT!P)}0%%91J1> zsH6ZUeGfBa!>?ffwxfNEwb`x@VYGII7NbP1{_#aZ02@o?CjJVmrot4cX8=0&2Ux8m zG0_wfW?hfGwNWy?qbl2R=O|u{L*7M}wxfUT@jDPu2h+nF%izdYZ?`YOrGQiz4L9#m zixdew>~1G|8wCkm!k_0B9p9`d+9b9k)j59pxc$5S!}#?<7|C}xWBD1GkkIRENwLr| z`Ia0e)Wso@pEQ{n_i4s6KalJ`gIf(P`fj*}=i{2Qr-Q0_f$u=rXU$KR?`j?UzxO~1 zWrSr%JCs4`tw5f4t@}v3Ki=T7R?emsc(6OrYnh6@_e<;wDvgLTCD>ec(28F2=)T4< z?5wT1NC6E>)vgoVb0LPWYk!L^(imj^GJi*)Kk%su@d+}!FAU_xv)7+~DEnnub}H!b zh?75UEnp>+K?A4hdoj%F;ztUY}XBYyVX zkIF(0l?)(y_aNPuO5}r}59N!UwD{ZR-u?dvDb(X}Jrn37dq#&kaDfXLoyxk!&w~%W z#qZIep0!-$6MG0)o@W~)7Kvdb_DYPr^g*7&PBU#QCDu;47wCkm#h<|h#wm9z{Qa_g z5+jX;a*49ba6d5xX#RcH0)xa~#>TW2gU;sav~uu!juP%r)@I<3rGW;`BG*A>f#QE* z1N>$V3+Yv}^r<0DZET6j5U$@F2Ht6Ith=w`pT}fInH6lZcLh|H&SG2F`O-*+KrXml zx6~8YNsUt7d}f~gKq3yAJEG3tFNF%h&H0Q9*nTX-^ZE5j=MP{>{Qa>!;E}-1B0+b3 z+sXlCSquOh_$S%UuXz>bg^(5i5sixd&-8?E@#KH5yxtuZ#puDHAh!|COKv z6aGy9hoey%g}~cB`^EPy;dbR~=07+7>;S^Sz}A9o zzftH}Lmp!TE{jg1@D@>}P7}i8pUtT_^omYJwWsqAF<^-P+C>)GaMW!z$$Y;NMeo7N z;rq1L*4%QTgTuKyiqnTH54Nlx_6`BF_|D>9v?#zji#FbkgAfX6163eZe3FYslzp&- zzvr6#QT~{;9lHpChnW+OTf@rmWQ2TnXlXYVWGbmfbsmg1@AqWYV_T&PbPj} z`@*dn;BQUVra&*K$ZktDhRZPI_34g#;N_MZ_{Yz6Pc2oz_;Eb` z3isn|UMl0=$4y~U4?`R7jSX9iJ&z!wQ+^6heNdNXavBePa0J7ApNV`mG`un))k6NL zj-t-~jW7W% zL2zv-v-k{u;G*8syQFPxe`yA?Wc58Rj^Si-Q1J;{tvJxpqPL$Od!BN@!h+oVo&Q9P zI6TsRI@8xQpdHXrYy8Q;#Ez1(uh)leW&ApbF_b|pV#bl)i^iIcrSaialOs#aR z78w-Y#rgP)+8zvsPP(YNGqB zC!#(8NtVMl<&Gw@5w@6Vyv(XC-9JuRwRJ?V#DA3u{e05SEdNN^ExUR|2mix|>6{ps z3Y*MXml1sfCm(nBqeCS;4NDLAS7zgsKxMzyMt)>8I{#JlLhba))>QUHAOUNED95z7 zQ0@LlzQ5rKn4>kLfq9UScfpOd@A6SMz@R+1Eub=x*YZT%)Kx(NG6H<@j5_;63eNs? z{g3Bwg^~dgC+%Y=ZMp*tf3-@Her7BH&b@rCH5j9KkB8N60E37z96bU?p~U#5(ex!Q ztwPo}UW9%ZAoo;cGGY##F{t8;W=qch5>9RNKC1cv1@kYMNfv`YPeE*00_yO)DOy?0 z$pD0>vpDd3ZoT}>XlPY1hVO>QwwCTAkW-OZ8%X6ba{V)?q{02zd(^{Wro;p<)c!+_ zlTjz@jTi$0K=swkE7wGqmQ-dvXR;I`XCG9}`4KEOV{Lqp8CETM=nUf+g{-r4vE#n$ z&`o{=95<52wP1hnJ|a%v{ps6AJKC^t2)J>K*pZEl2cv}xi*x=}5+*o&m4Ga_ets=f zdv(lJG3#}(^~IV5bf-O~(Rpc3=--}`J)L9|mx;1k7yH9Gv2}US8+Gds^NuZG8 z?E;9CJ)Lp}Y>>f$#d=S8xjc1_xSqV%NPC|9CAAmkMkBQ~lR!depil@=1Gjn3{+s_L zdM8RPuq!#=$-9*rK0lo}I(g3{Hiuj@i%j;@KCqInR++ocUll{ex zk5fk@CKgvf@|k{y_vjn-os1cZG5>?%)1QHh{y^66fT^7S^3R}_2KR%Dv3eLG#FxyT z4QP7T%;&78*ybdrMzIV7EqHyhQgoisQ03%I}9qydIu#zphYQ@$R$h z#5<={WE6 zVX3K4MZ>becOS6i9&i$ce&sL3`~Y}Xyjnn}D#bX0Zhxw52AA_D*w|AkTc;HTb$XwT zPN#CLo6W?^Dg4Q|7Y0fO%x6#7%yRUd9K$VOK4d3Xk-z;`}54ZN3)pawd<+$ z-~2HNGk~&Lnv(b`X_z(kFeruu?d3Ar-;3WF&S99rQm>S+r-o8?9y09?96yPiZm!w{ z-zLiKS`u;EL<2!GrSYZ`vC!==k#=@u=p$7-<_udCWrOiqM4Z?GQ>O#HF$W<)sRUhw zTcI#Or_3C^14OYr;j~Vt2^f!A*R7Hf8uZH;W0+Y@q8?byH82)XC_#?SJ1Rl!FzXh_ z#8cymncqrF=A7Q^MBY=BU+M7hQ2}6j>ZRzlq4`z|g{K0Yug>Bt436v5@pa`T^3))F z7CGRKEf&LOVIHn$_ulJwwc|m_$g@)K5EWEH9bW zHw@+?Xe=To3jrB^iQe^vc@-_<4t2%>g=t`nW7)w6*IXkPGd+ZE$(N@EANt81?M9hZ za!a*hm<7=DbS$j{9^wtjI?fW%zbnldJM6;T-;^bQ9`}cm`*dZd$7dnF!NB6f&jzHs z7YgtstOi}s9Q3}Uq(;{;9ls*GOxuT+M9fI5+412+4^|xB`W{!jJ1UGe zzel7XJP4Xf1OT+>%*S;LOfwIiKGHA^*Y20bvX=4E$Jfp~II5R;Tm&ynGltuhDg4oh z=;Y*`aR!zvfLpcI&SUgjHs_{-)d~yJ>7OXA&X?=l%q*H`;Rs)r}x z+f!y=Pffhd5Vk4*9wiYNGP{CPGj?2wY0I8A?*9sn78#h{L(A9R6{O_1_}bgcKXZ>I z09@zbpQl$Mo-(mAjmZcbc(e zXcjXoO-7`xX=b^&52|d|vJBu|k|CM)5p7ptf{sDxLve4>C!nj|Nv#*)3-%8+AIG&D(K0JNwp6zgk@<$sc zq+zoYl}ao8E78>6DV9w8R@rpca2xyEp2FsglZfd}_g_j(Tfthufq^5V`G}>L&jPJu z7lXovU_IWgGS;f~F;K!Wh&dcL@fDnLz*Kt$e26H!6<)jhWOyoZAw0=J2z7O+*26<%0q>qWl=;0FV;SucvIw=8*79+{++5Xb`SgrE=+S<|4eDSWO2>1I@Kwef*_FXtE z&3Dg<&r;6Wo3cl#7S3G*w9iDmqLS^VSJ#T*GEdf9GSt@>f3)<;F z@b}7TXf~aEMh1^n&Bo)`8aYd9WphNp4d%D8&TMZ^mG?9-(fb|f(}*NbRgRyp;)C=# zbtsyh;9E@U*Tw47=RoRIi=y#Mk zlfDFj*kiOm^VuLbb~|6f>&;|lUG3#@=1wTYXSW`1NQM~i-< z1hQW@nFctQsXY(&2+cg-$m*8;sa5n&Xq_zysnjJM3QD;V8#8H{1< zUmob$D9mM^w-UVjRlw`8J2-wp6gbSNS>3lVx1Rk*hI_9Zv&?)%7qhY7K@CWn9yJy| zB8bn{<5FAbEPFdgWm$_s;pc1!+_jyYgsynaCsEEF5}gpTq{6=vPO%sbWE*=>G^N{T zY?nwOxPEfzXMS^@bS4%8qsxdjri2ogjaLXysu2lt+FZ@_wCfXMWCgAV z97vkF#WKvI4|=$s&(+;T&=0}fI?PuG4kcjt{Z^u3)H*^fVchjmuC_5savLmtJ_&}) z@A^e#P!CeR#1#+dM?$IZ?S;poC12G3i$)H_FPDER)e1D&Q)5Q6=vB&qP@Y8&361mL zV5+-ZG(v-9@;HaISfedwJ|r^(BP8*c!@-l2TdDy?2bd(UOfzz$^6J2<1RhNq5KJd2 zrwr$XL^l*T>=oL=hou`0{y6%**j5#R$a_JL)fPPVNH<#Vdiv&6FR-pjBt!>xK9uiT z)_GS^fu%x^2P;whLAf^=JH`j{ziuKYFk@!uZ9iX6V6}`%H*Z@^Q@nAJR9Hsw;F*|^AX1VUS1CtjNY1~x!e)5b5B5l0 z+1|lwcr$_ioy8#?BBWvVA2KxK6Y)aQ*H`;{JAnHMRVdGK26c+hNd!_`@~fdr?HA^2 z11)cHB6Q!TvHh^*b@nG^d9OXE>2s6VZV|9s!;aJ??0_Q`5b;)%Oy?Y(5dZB<($|BZ zG0RoDBlq8r%vgHo3jwm`{gQ`JLnWdKQQ_)iahYcJ`cwq|wcUi^3}DsSm#RqT8zxY> zfZv4GD5GKwL{~Bq3~&LtU2&Xgs^WG>kjGdK)z}2aF0O_sJNUQ$e64j9+UoXfy2^Zt zlgJ>z%k4!#QIIw=D(Eb}GTNE0F*~uQ{+FP`AfJ+l)v&9bpKR`I6wF-hc3OlD*IPWH zCe-h8p;mt|?0JV#0X0+pVzFqKYIW$I4)*@SRS}i+)!db_l^erAfhT-Z)f|JIYycOh z-R3MSB9F1_&rcnm&)-XCi?GqIwm}$Cj-=`XjtOq$1Fe;438$YLwmhtJ1S59$4Il7p z4urJb-$XhjD*wUd)&o|=K3`KR=#(h=_=lT8y^y@E zxcd<%j3;PTNC1@=ByY&)V3@R$-q>v2)jMMRU)mZfT)4xq7$+1eY8&JG?-}c1|KV{V z@axTVVIImgT7?CJUM(muB65~hQtrs?A%MH^{7_;+lL(JZ(<%g}P4ZvU68QrhQX$s% z!JE8?I#@EA+Zov^1yNB6|Gu4+8?J9hgAs3jIek5vYYwMLof4`BC(7KUQhB)6_@jPw z-e3*vHZtffoqk{=bx?>8V(?n>?+&Dtg_0u{UpC?_S_5Fu8Cvu&VZjtfgL8CrjL+cY zoDKO@^_8&OkbWT?rdkoRnG%0)xL!SfDOt^`QE1u!1H)$T1ccSTx-jmQL%Lx%SR9-sj$T zwPOVU-;oIzqNIe7@M(hKp0@I{p7K{YB7h*<6BNbUsMZA_IFt+MsiQYO3^6*`2+ck) z53ZuM-~SuC4J&}`PI_JVTRxM^zqagoY)oviM{KrKteSxx0~CJm-gXFvM|3DKRxVqp zob^IU0qSJ&uFtO6miKta?eKq(l{9JNrKk^88Sgq-jAZkr^QBPH&5`d>>Pks#nT zY4qilum3*y??rz@^%UpXpip5i(^u^({+p}=?vrGFj$^jS)#as2?xvV3&B0fG2@H<_ zggI0oFt*sF_CUvBHCIbc$!F!M=OU#x!sC9Y!~fxyK;?o8`pJ+70nWyO@8}XsMzKoN zY^+pIjawnc_ErS(_9KP~IGviAoXu#$u1}DgSz#$s1cuslD}ZDd0#KM?tGz=IY0z#J zR*HT9H(~}jX;6vkOK}9a9NXDf{~`;&3kGv#4er64E+Uu2aQ>fA=IL`i|DZpOwR^+< z<)S)#Z}Xl{2*C9`1%zN$UuLNQ7Tq}KIAzwq(T_$NF5GEY>^W2xjMwnu#dkaKExz&n z$Fd`J08)7Gc@#z4VeR?0U>B)KcEby{rq1Y^bJ{fT?Ji$u=kZT+vgMMI>DU3jg4vy57t`8iXbsw zJN~)!a3EM;!1x`d7x&@CCzq13SC}-8a0bj6fi~7UlV9qHb722I`8P^fZg+g@WZ4ba zbV}~u&DoDV`G^rH<|9}(?q|1w07Bx}!UHft5ex$PQ*)EC=a>a#9zVpj8hLR)c;T6G zGcvv(l;0)?Ahix$WnIOycQmU}yS}skV6Q|UWMNEWDrc+GU#8PVK5EeGwQllq?1Lu5 z_SpNA(IEvx&Wrk_7OnVeR8)F|tJ5;|P84~J+BUS@0|eXb}Ly zERhlu1PEOZ=>w8OMJmsj3D6||bpSk1DWRE$Jk{qH=|`cxt;NHC4k-7iC4 zNv{Bl#w}s_=d54(W3fES{h~0o=Xl^P#=&q$M>){~&1T!#(fcEb#1v%yzFv9F^MK@p z_{h8m*I)9mdjDO6x3V`C6HTG^By0Tqn`(Hcs1B{4^$i4PS(*P~u9S?h#W0L7Tt?Eg zG|1mSR6oKh>o9$c>>~!l76+Zto^IrYxMPJ-R}aJl0!;ETfc^xk)%~U10pIvu?gp&Oo)k>)LxW?Bj9hU@@1^WS*WrJQ z<%>+(jK+N!q2?wKO|+@qb=epO5NH<+59msbM;>yE_YWJSD|YF-!rdK$Z`VdbMEtLc zZzc^h6|7a+9CkV~{*&`GTIXMV57>(l#qCo4_O8~rVp|SbZRpi)n*76;3_YR zz>}Sld$=|FOg(1TO`LA!{U5Ze3#~^5UFZocvSzeA^6}o7fKPj_2K4uV?Q;T5>@J3` zSX)=!{<2GwlG~mZBp&qIXQ}=;YzoB|2NuiZUo}@Bo8lk~3&OY}Rv*q)_Iwzmcg3e>UZ!}LbH z0HYZG2tuQ6K(>0>uqTc-+U+~oEJFJ}1bnPU{6$H(17m%7L*`090vD({jtp4u1O69p z{&Fww{ka&}UhF+{yCnPhnm0N3y>J+0Y-&$%)Nq=+{C|chW`X7>bN>wFlhQtm4n3sY7~BT%=(#H6+%3# zLzY1Qcbg0Qazz988$?1HX8LG3o=+ZoEU-BB{Xt}sfwjKVLbN}Y z2aq{&!K>Z}VYQepuW6%aJ)FN0qrN-$Q%UF#b%(8mWul1%yP!dI80blpD^#3a+Z2jS zi41%ANS%;Pg7MXlf7h=jgc;fgIA9~1ZvVUPUx9))*nCGYn#gCAh3m^Bgb6922?n!= z#zZkmkng%<$;}>O1Fiv}nd?z!y)j>YI3TmR(LH>BgeY-6C%5K&J5>twfeGg)u_xvB zg$Io*#ZugwZ-(zV==I2uF1T{&QHde%ZN&_;kXl9S_a@%x~S7xw;q>f0P1l;tP zJ}|k}^`(VHosDU+YOCRtM!9sM3He5MaDR})_)U@dhgxf#GW;&CEcbn#h|~VVku7;) z15S|&;qS}9nKQiLmz1#3f`=EULH^!A`9Gj+gcov4ap0fF;3t7$a&_1c=u3}so7d1Q z4Y=E&q=iVcZalFK_LsvHLz?6&TTe=`GC539{>BMxg8!t4X1S1?Nnj~9la?K1nKdt< zXFi{;4}u=-GD;(xpYFqBkMAc#}Az|ky?tg~!%yM>{b zH;VtX0Kk8e&LIdaxqww)RCFm|AcyYBsi*2)f{3i zc@+`e?{#E>yRpJ8kG`3Lq#OKmEQ2>;ZI#DG-6Tw0Qc;WyN9%DNy#(#o>gRtx?<<< zzi^E2b2S-PVWoESL1_}{40QIt0PIhs${|whE@ckmhZOAQI#zs>sy*&ZH*|y3(0Szx zs?>td7tmuJJCrqYq6ZBy%#P6zLg9LcHNB~z*k6RL1NMC-ItqX58Go3@Ri6#l>y{{# zrB1250B7YA(#f4s&30D4Z33JngnEhuLAP&7_b)(P_D;V>NK2$!<_`gC;nzf^iENeX z9!>t!LL|?J5p?3YSSAv=oFyrOEf3{tVCSr;CNnlvfSbd_d?jhWUiGWwPVnv@rVQLNoLtpmZsT6_D&`(|fM#ev`blVGOvx`N%%XVUD(0-8X0VA}r z`{SP30(}U}-Y%#n#glH86;gpEX^<9d0SB#ht(mWSpX2%rss1-xvV@KYaET94I)rRw)vZda0S5a14R)uK%hZK*|V+ve0<( zsxPzDbc|d{-RgakJ}~=G2+YEDH+8D2RHpZ{GgC^k-!yIX`BVg|Y5-1o-`ead<|wa8 z9M1ZaAAAost{RXcG&p$5G#PGWMQ3p@?(GC(`Y~-pm42gK1+tpH>{c<~yICFjVTQyDMumuk#^<8?;vYz-3JR(};E!iWI=|(iK zx@dRylZF70DEbKSxm2r#K>hCTB_CJf7`FalhcL(x5g@c8$2kRv7A+6(Tw4p#J`-Sp z+r$B*PxIpC^It2xXevVc{F80+ssUF>9k=vb8Wp%xXWtBT781?#m?(pfo|9i{D7m@r z0v5kGCkmv4_jxw~o>o}eA^9NqwO2xnG9$zgc-DESHk(Z-R=^S_| zRkFGxWYr}Iu6zjGFpX<9H49SPzq3FIC4_^9I3KPi;c@bOTnxZWth1DVq2DgMHoPTq zrFJja{F6IPlh+cInIM?(H+9eSa*R-66> zZa@f)!*>1K<#3;NxfGPtIn2lRh*;52kle*9*vszuiR)5swh_AmJNjgbEM}q9l43sUt0AzG} z!if7{Y-U8BbtdTHJmnprmz$RYV|{ncZpddCA6M=}@``TZhd8v9y*x*;DHd<+PwkXK z^BIpUvX~qUK+cFn1dEj#+VB$T+#x}5(ePguiHYL4aEJDRd&GOXQ@~VsZnL@p zFzt;LaM5+D)uh|Flol`k{@NS9bA2%()fPgO0yIhnG6x+C3Hat$dg3U6ubMYDlcu^@ zzzNXUNr3*pSUaK$}8K9?Zfa?}#^*(5*r zsp~m6lgMZ6NY{(wGaE+Mzr~&hE0{e4dVcF|I^OYTJprfR!3EI=ktXCyGUL(S^7Q#I z+s$VYxpt5ywca34tCEv~i;a~6BSI?U~Q(<3@F#pMy)b z9?IX$K|ZDvI9T*viXjq@MwVY?+O!{sw1|kW>j1lxf?7I3mi&UQVj*PWs1U$dt1F!m z^c*gG7<-Sb2n2cMfK1#`G1HCB`jCXC;}QVT-rUP;tpPiU0u--q%zx;KzCX;1OG1D~wR^Y|i(b&K8zLI&#|A#ek9K?f z2H7om^XpYUO&*q8M!6*P_=IROJ^6UhU3z2!ichvFq^!SKOK&_W2Iu+IqeVHYC6b!%SC5Fsc-$`88NRWZ48Il|)p9b5b$M>`Q@-L- z(#S?iXiLGp1xG=@|K|ft5|Vt8orgG#O0QVNY=&HORYy8*uFKSV^KJlx=hnJhE`Gm? zut3WpDKw;OXQC`$kK`4!9oQ^+VfOL8g-5a9s0_rRPdflAM4D8uPx;X)XDyBa|E-g4 zCg;}Wk~ZPSeZuHrR}@oz$P4?C$e2KbwVd1^zrMi2f9W=2Oqgo`>53hh#~6M5Kwpv& z579F4l>zsJSg{R%U!G{tG7_pLTzG8)|4cTGfY4E|X{+-kGFL{E?ghye|f%wlRv^;Eos z7PvBH$|K+k6S4g0htrXje}ps`;MH&Wj3Wm`m}2ucgJPJNSmC~(Ae#dGpU&d9m7HIE$pz0E@DlD6P0Zt!u&Y_I_oKV5UVs7{@VVf| zvUJhvctUKkwnHzfw3vtz+5y>}_O^HP{_%ualJ5%><-~m$(kgn*Om=-i~>qA;O zUIKY8Cc+=l4khwHX-8zg+gjfd$Rs^DN=>O=8#R2zJQ@}jmVrnl_fBkMs%J}Bm=|~C z<<{Xa?z>mQ&QzEI*l^Wa6ibqYaguggg|w6M3u=IUw3CFN{9%(UU|KpT0*t-wS=xY+&s1HV4Uuq++( z47~t^BAai{X+e0JT^BC3m!(&1j^}S@@C;huet`p9O;b6oa5n6^J83bPmJ8roMZlPo zBJ$TRF8;Xmkyg1N@z}2SD|P5X^q9wJsI_F|Gb6h-LnX7Z{ogXrrBt>KUC%|ve$mGD zpTq?0iKJkP&;07hx1}R5NibytJC6rh8-WO6f8kc8)o7}+1ovQTOuqE+hc+AqhPi4sKoa6l$0AWWUuPvV;`~eldsa*lq z+lkkdv#(*^&L_R@FO4>%8EetifD(oWF`zfQ7DsO%Iu1VlGhY^3t^S$~cL(#BgA1}- zTj;d5P$T-5k4xB*@oycgPh~mV2V1e;33zd+joEMy9=7k!SxCSy&FsV?JaYPz9JUji zkbmC^0*@;wRc%ojV=Ynw+R@1+(0OuU`Mntoh+$ zkYB$;`H*!pmA1GD8xzv-CC&D$V$AV_1rcyXPU21^2_crX>0ZHX`i?(++VMGp-puo7 z=0_!b7UORq?G-6oS?y2$xI5cWu!`MdwujSqvCt;fBpM9i90G!GOWTnQspr1FVKc;B ziu+5wOt=0EstI#UXUl*?+;o2A)UafCGQCmy@LgKJNm0`hIP4KR7Bx?QMCA5lu;~55 zT`bilzU>=%LZN`GP7c)yJfo85gV;{gegj#_6HXtZ&NgRT2 zG0GR3u=jcd`a+gakI@f5j|&IswuQo8H1gpM+LA6HAeTSY7Ir;dUAX!RO{k}FE&Jtg z7s$j(Bky3w_a_<*IiK1u=S2dmx=1p)q_F83FBIy2lIbj;0u3G0!3bdPqtO$~yY-=0 zGF2z&35Ogmuz1ze%6JtEh5~Dl;t8KS6mVxWp93k--v!^W$0}T_QfjYCO_h4DwA@q} zHeJBx=2NRy2L8EJQ8as51x48(sh8D$zc6b@i&Faf)%Jq-a+LzP+{scC= z9DM_tUCw7VeFmiqxPhJ+b$or<%1wWcJj>&Cg81{W>ST)%-uclGyk7r|_3=L})P+J@ z_`=M;Ok#;Vf;G&q9_P1pY1yB0z*MsnK^y z6UQi6WtZwr+;fpzYN0#H&Hituk22YcAxMW2bkE(kzQ2@Hym}=q7rrm5f25dJbzsmP zOUpKtOAlBf71(dhjPl$9f6SafcR`eT=AP9OxH0XJY!p9Ycv-8J5J?3LF9z#&e!McF z!l9G5jP>UgNEA(&$l}GOSA0$4ouN_fU^R9=C{qL!T^UrHa63|f25o!mPTh!U7caN_ zeh2k_Q)_UM{f=f$uxl!Dt2&|^5BrgCO&emL0Aw+S<8K%wv_OfRHEJ-Q;kObxE9 zCo2>cCe$GnCMY3iUr49z-S;IrBIu)fh_y>^X4Nc+#!N^{x{9PXA|Pyl9z<@EJP z_!;z|kYZTG^>vdfu+A$TwX(->I@tz&s6Sr;gIuQee4YL=(2D9AuXA@l12)7<;bO!X ziz;R2aUkVv-(VR%PRU|Q1O7Rkzyw0S@LYFiAT^M82MA6NYPHi~X^AFeh93i3!>Gm} z=uU}~qCj%yd3{_dk-oKB)ae&~;XIF?uN7?qI=ce(!fR~hnXt)+MDn+TVRtR!Flt`5%pw|8#>@X3kdn)9bc(B-~prg>z|$Nr;;8dHw}pA+%oT-ICstf z7ab&{zq`vO$+4#0wSavkU6UXB$a~&YuZvnGa!0`bPWi1D$}zrT246QZFE!Fji3_zL*&oKmL<`Z@;%Ej;mxj{O8ZDTx7Fz$-S6#ILa#S zJ)fX=sIG7FI&Ioo)MBVZNJa=T&YTw8FewYcYLx{EKwLx1c zKi64@S~}RRmxH9Bbez0;ljT`>rs8nawy$4Lk-yb@!J#91DoT;@=zc$NhHmJE--CKAJdRZOp4yOffTY0Z7R_0D@ z*GCI0gCepO-^=H!Dp-(kSqYY*tR!E*O%^l6rd5@<_+eM+Y(ACtMil5IR&+TZy&TaG zl6YfFXfp82sMS!*B}pqA={)FS2!6>+47A5s-DYYv>JPJG@+lDot5FH z0pG0l3?OI8Yi>58*5vm-W7WOJ{iebDu`!i!Yw3*pNWNN$d7NkI_sh24&Xq&qxprKC$x8tD${Mq26a?rykiqvyQu+&g!Mas0r* z<{xW4>zBqSiQJbJ+0O1(!HfelM^OrbF$dS^Y|l4!t6lA-`m`!I8#Jp}mw?@%*TU^= zhM3E&zqfiLD~Z%iksvg~|ORqdVc+>lghq=@!c{9kE9@Y}?j!$zAb4AVG0>$aEQO!p}0_&GD zGJ`5h?1Jd&+Zr-sBOK9w5l#{-G)(YA3qPE;vZf+ZyuCN+!t#6{WNHOp&r+m8#}c)J}O2i z7R+>9=n_pLQRw15U zbbwuiV-OdoCOqLzCW{4H$6S=}Hb`D>_~AEPbbP^)!7pvPXInP5cTGkNnr@A)OcuBP z$PU4Yes&Y4aW7t%o#r_gXt|q-VXmm|6()lme#h7;e_8B2XL;o^jNcFS*8<%+Xco{i zgU+J_Vm1_B^xm5Wof!;toY=3Jun1Vy_1rRVl|!6FDTFKl91+vEDW?|tptdfoW30~| zh^x~sdx(7$=Yt;yz{h|3un~Dq-9cEDXZ!?l=qLLVxPc$dmXjnBo*H}KNaaJ5vILMr z2`LXfj$Fmb!2s+xkJA*+p-(VW;G^Qz2`6Hw)uQ~eDFtbtoOsa6w@J+xeR=Zeaf#}m zHMyCg_+qWE)cS<;CV9Pp2YNA!_Q%Upclna?)MeMFO1B)@9ieW=kVHkPpulBw zn6ZH6`Rl`CKW?-{rlfBm2KB)xsv_=of&+=!xSIyD+ajNZC(uAx_l2b9&iWSg zgFa=Ohb+$Qf(@_7sdX~u0IUj`oa5|0Mgqs{2=XI`)6EM{P7R)oWQHxIIDG>@43-f)vdm*2qSv>L%>M~_An6J-xbi!OA+vh z-cVGPc|S=S87B=}hUnRT>|>?^1HVuC(pq&E5l=iu?wX- z$($dww7=(maEl*+wD0YkGl1-1yuVo11AClt(vhwo9U4E3f}sz1F(Tn>zbqG2ZP6M- zh1Dv{$zt-FC?B~7SzM`XRvmPYc(vJxb1Fwm*@-ENKY&q#KJKfc)l7q*)2#v z;PALBe}F^!!ZLbkm{&#lJq43`F0Ey#;6#~6mFG9J73~6B(gYR~-CE}ug1#iH;Kvdb z#^0I~aZ19WXIrWwR0!NL*!Ai18e5bWB{w%`=7|U4M@nk1D!TSLDtzB5*E*+-QVq7d zTV_{PYD}5VqzPsx5d7jM;~(0hDZKdu)Qhrdb!W@iAD~ z#^7F-0I&pMnm)O}2}>n!qIJ$Uew*0EZ^VWyz(10TqOF}Qx_Ep^R*P#=hv|_wP{BnidgOw_)s|i-TE-ZMiV&ppmiR~lr z0jyUke+=CQ(g?SZ3$M%LgYgd;8MNV@G%NE{QM0k9eN%F&2p?z{S}q+{Mr>DBy0`RL z_H$X@844~b_*;a}b*SYL=y$(&N~v;BMWehV@pHMQMM7FUtFN8glOE3ZPnQ)5a^1}* zsm?=nM>s|^N2hkV=<`Ox=y-xiI;*61mZe z!<9=_6Oz~Nl$`hT_m)BQhMf3sUYkQJOl19T6LQ$$fBysA@v~mBr}_Exb|1Q_&lZVqFWh2c@J+}~%se2w5UYM~>gY@tZ&%S8MaL3JWr*Z0| zEvYg%clF5Zms4Tj+S`)E-%CHmUILky(A9gzTQH1r6+^6o30TA?P(~loBM|*O7;E^z z!=KuyQ9gkt=vX@fr3g~9ryvl7ZgwKnhD=vYjTm{9iou;dAC2}s>2K0QJn>47ad5=~ zx(EZbjLb~rN|9IsX9PeuVR?|GmB34N*-oVu7_Vjm-Om-XR@$E6iQ9e`)$iPwPZO?x zK2(@65pgK(TmluA&Qu~vWVJeIl>tabQE&v}L6RP%>4(NgCUP-eB02vPC6gZSL!-Cdki@6E+ z(np8&4_E(cR{X~4eCMr^?mGIuyih4r$xHbQ@Q=s&GS~~NzS`sYbqSr$uC|@q>WbBs zhdW~>XqSu@%6F+2*=-N0zQ3(9O+uiak`S+8u}(i~vHeqzpE&A!DdVT*JNrD}but{X zqF$^=qoL~mRa7)dA{mq!=N~#gX-p1z8AbmRBGp@7~VM;L-VC3102C**lEHsK2ZydH2f++m%aGjYX4D zj=*QZBDjcTsk4y{Y`eo^9RNSdH0`O4QB`vCx*RXDmAk#ZSWXQhj|?U8`3(B1dcaXN z!+xp5Vag_y77$phc^sx{xo=)l2)iHWkP5ge_FBa;)J$CLRLz009y1CMFg@`c>C24cSlt_XhkM6t=mHV7fl z`dsvlS@*{q7^m^Vc{{%0<-Tdy*!Z4Y3k9r4;E3+&y#`i91Wn`^V7F1=qm9SF?HV&vLOlr2o-$*^|M^uil+#E!Py8A)=ff%9a#3>7`G9+p zfP+QCqXS-yxVQT3dJoyObWSNIi-PqI`eYe(ia%%Eecp9Zh54771A;8zm#XHxqoSh? zfv%oNw;v1yD=TqVJ*X6(Ir_#V@YdW`dQSl>9t{rpHTeTtSwD-7+GRH;E0Wwc#vC0E z^8m$t)^&L@_Y!8nNU61+;N(8vhYo*tOtzl6ECbR1qgyX}|0)`4@5Li+x}eeNa7aNI zt>xq(ldxw4_zaZOH*8^DJS%|K7^b!>Db%S%E*oU7cLV1p)y9VQ8(lX*W(z6d z?jT1)ysAVV&Xuk(JnHk#{>cg=xMMso4&_20f8?~8eE*11PmX|9Cu8#|EzGUI+?y~I zF{aCZMlsHYsIL&4eC7Od>@>WGWK-A&|9@z0;9+}QL_O~_+z3idzf!r(jA%IT&8CO< zC2;kmT1`8R=>oZ*YO_2NGm*usep7$sw9975lnjXOSH2lA7u-qP0l?x`b0pwfEu2-d zcnCrae|Eh33vv%48I%Md?cCx? zL@kAX&>%eI#ywF0h9OX@^8J!7nMN@4=N26pmQoO`U2ateuZo5Y?%X+R?%Gs08`=M4AlKW~?y z{$|VGmG))6aJWr^=y_z*MLlY4#?jus)ulwBHv!>1$>Z+!2yu8}7u9T^TI@Oh_wRU* zeL!v4_QTWxObPAM8R!#cI3IjW`4@M*_+~;VPeq>tf;*^E;{}XWUjwW<_g_|>%=K&| zJ8Gq*^T+cr!L#Y3!!8=Km2lxoD~~w6dUyUXr@h0kP}kqH84oVf&%NG`aa3O zr=-KKzYnyA+ys%kYuvC3rs*4hhF`fm8GCv2LpnT#&z+R#8BUr~(97M)iy=1QpSezX zrZYcmGd-If5+;rMPfqIq< zV*~)5|HY|8WNbIi5AgZ+0HcI7hP_5bG^p)w7oOuuYwDFRajk{G;s;`=|FUgOE-IuA z^PnALfl*(cthEV600Z#S>My*ntKa0Q_5JdZnD)8c$leT3l+JM3o81hLbmFuaq$In5 z3p-DT_y`z~=&#b(@DGssEPiCj@V>dc#0~{bH)^W`U)ctPuJT{>n%`4|PvB{Glc1gw z$vgbj3fhP44#MAZG&SHLL`riK8#I={5CGsO@IDd=S;s$oQ)U2`L6Xz{BApU3sZdhJ zV4*AK9-E7sv$>TJJd&~`gkQ1Sl~j}c(!S!C0~=JD%gzUj1IwV!DGtyufWR6z*fLiCsh}Sy^pk>k^Tm+R- zudX@_hnK5!q3jBvn?DP6YllM){oSYDJh<~b?@>tNdG&EOoGJV!hL7n6q@wU@E`MbB zAsfdUzIB9$m;1DYU-j

    799~RbhmB5VrE|>5JvD@n6SLw#W$-=h(RL#okV>A>T3NIKS~_q9z=qE2zE7^b%4M3tef*eKDM|TbV5*qRizAPGwYSh6y~W&?;VX2tnr;sa=}Y18TS!jkQ(zC zKf$B!#}&lZ{sP!D%Mw!b)RrMKG~9jHIm+bc_F^Bm^OnQ--;EV*}Dn?kMmebgJlw^?XmYcL%{e9Z5HDf}?6(a_r))Gk1> zX6iTg2Kc0Om#0u{@4z$-?2hxGw&Y&Yz`1u}FxGeYhYX|+heO%w+b)w!@z{M)Pwlzv zMB~cRK`{XB?<{-ibA1pLX3&IRHd@A%FDaGybt*Pxn4#6M?y3?N5h9wG!$ zSNRmY0nT5!w`oW&*Y$Tx*upa6+`*&0b&@xo;bb5!XS}0z+}@QBo6D18Yg%()bqA0W zl$>LTpT`1Odc7daI1@X%fb*p6;J19Aaaa71))CX&J~W|F`D=T;GNRK$??!? z)U+=Ik9gKhmer)%K5CFqNDw6J8;D$lt^B>#m2jc&q5hkJ(0NB{MxtZ3~Zcq)NZ zA1KhrLDJkCiJS#LByDo&`Gz()+7k?$F2hJ1F$uUc+UwxA_4w5D`Sh!3O(>;va%>m9 zhCfO-;`6C5@nd2b% z`Cc#+<&gmISd zA!_Ipi3bH$s0&#eB>QZ&V4a>$*$(Y}_-)$L<&rdHaxwYHLARC9Ve_1k_@J9zH{-2& z%+p+%W@==?%Jv$}Rc`Na5I|6CNiZ;G7N;TD?EvqE|PnoiOh&+74#b=LAzH z-+wckw9J#485zt;4|R%g0x4hV zavhEOgk!J`wi&o0gpP<6G|)q$YF+kX12;!b7)J_!$g*i!5d`n#r(NR;>feoO)2;OJ zj`&A)l1kDb+=8v{ech=OBDGr?{i|iElPY&}x(4xL@n?IyaM< z?Un!F*Ft80$_G3ROISDKb#J3I%yHxEM$Hx048Iw+AFe3N^$CdhtKynb#2;st&V4sN zpbrYgVs?x7mxKFtK6B+9SIIK@&$GvKlX$MA3vb(zB+ajr2r#AmHe0U zl27A4ulgP+ya~?KE2=xtwzMMHx+tu?I`L5IJFb@g8YD`!7O1)48cs{E$I_R0V-@hh z6Tp?lDPCbti(l|jND2#d<=jtqGLf#2f~y*i<@Gmi#>)_@1RbupaR%QXD2w@>1J=Vr+2hoSfkyS^Wk6H-t;ZQ@Qkm^0s8rytV zyJ?BtBmvw}NuIF|q>vo!DTZlBXg)@!pnvGEVo;XJjsTKQPivT6-!4gaNS0}iFssvQ ze6m*4n+K(nk-U07y57oDOC8~3orjDRo<8kdy|G?}F1DI!0!LJr&FYPS55bxu(Jj90 ztijee+WcJFDXPt3L+s2)zramEL?&PYrPU*fZYt4{)j5e4NDiXVCwlJ7f`ufmLEsnh z5}F~Dk?$`r)}Tz7FNx##LB=o&OKaP$7eGSNO`D;@X8S~Uud(cQ0SRD{$Dm_8jXE0G zZf@9&h)Z@BH}Y;3C>iWc+OKxUOMD8l_mdt)%n^+{gcWwa97o+ESATnVi+FisDe{=KBgxha)H6m$k7kS3f^&Y^d&W-kq$~mW_S#eX_B~Mw|451`qq7ucW1J zMTuWxbFupglia%qSl=$HGXOD!&H2pqT$zTJn;m7llSbo&Y(cYB=UQDC6d4-Fe4*oW z`jA!U`8pT;wIwOnAD_>77~E}apf|RXFc!&pGp&0Gg*ldOE|e+?hY`tEj%($SkD?|% z0WoL9)#%M^f2)!1y*3Aj1;((aofT9H;Ak`MO83N6stdC#28z-c*9UlD7s1FjF*&$0 zWh#+=mEiG~*Qf&((bZTUq_pO8xDq2tfs%&VPo_TdbjPBq#q3A@1F^kbEi4=@bU57( z3OajB=TA)$=q53A70Ft=%~_-K@eUk+R3oZwOcLEIv-nlDi|yW#ooG{nv$VgYL->=g z-q33qi{PjHSPE<0*26LvpV>ZJjWEM88(Ad_`htl(`Ds;N0^ELo$6|IBQxZO{#8HuE znN*5m$}LEDthH!r)zv=!dCh3Heo0z^}qw|x&1Dl97B{K$>e(YtkUtF%(6YOl7+ZnP2owlG|B;$0V2FFu$^&kqnf-xqJDgu~mkEkJ=URIf}r7pzdca9oPdJ_2>5Of19lvo9_E zv>~pePqtAh++}u^B8ESrp}C{%AFocoE~J#qWQS2YBYL#p-}f3 z;yGcUN%bOQ$iHD1O7|?SWXWWCUn2)n3h$7eA!6p-O{i@I+B zO@G!@yk7H@$mcWlXto@JvJ>9SFk+hh=}xh9itiJMMRkYWl9CUVTlmOt>0Q?Rg88W^ zoD2q=bPv_LT-0AP?{t1+ML^!RsONdphhZO6hjvrg|8uhe$c536&RAsIf_&!y5?+%l zTai!Dd4wPj-u^F}TnnCo-m2VBgZ_XIl+I|B&qoCudcVBjLVU7YvqsM9Pl9-cL~}&5 zL@m8#jVQG$-Em`HrN-$n_fDObbIO>Si#U@aVJy4=i_A|%ay6*1U9)HdsMlhP8m^Ut zF94tw*`nv!Q>LeK-XAj3KG7-#!mRi*S7-bqNnHYm0i&!yD%>DbFq?DM!sG&XI!??&-+~Xq223qEs(3$mx_Qy0O6`)?2fhIpZ3h+xZ{bS@uiiERq zLwLzm*X^ZE$8f#|jfGmUh1!ALFQaIuy+xt$o~_$esonorwK zzVFGO95L@rP|fZEnC5p?R zB@R0QziXKI177UhFMzQ>`hMi=GTL&=afcTEh;iTc5DUWO3>p$ zxZ#h|34XmjPsOIHa`$xl4s;DSq?@P3LLRv6i>SMp{H&iX*tD!)<>St`{3>y7rzvXe zP-YmfK zlrshAKW!V06tTu-Pr0y%fheF;QMl55VD`_gX~TebiMSeCdlfev@I4W+{U*MKjCC{T z<8NC#6-1W8jLvM-ncDL-DLYiO@n|?|55p0^Gc?#Wf|JTPeGd#rhK#9M}=NI<>MI8Ty$ zj|ITsfN@F#=EJ34DeL!1_5V^(K4|e4D%Vds0v2x@t&N<}5roo?OGH>%q>N5j4H{vL zmrN6M;=G|hmy$fpd%+f6Vu{9ZaC|IK1aS@7n&#~*IQ^@)ot_@2+Y!z$UzQrJ$Z9_; z+R%0R2~|pm*>QddN{5rftO~N3vN2+_LTi=hlfGx2;c@?jKHh--{)o@eJ=48f<8G8D zbhA@pOs9W)eK;tN{S%vnw;QkrE5NnzyV~CP?fWU4NPd@haU=1Zm_gfi7vSh+I<9T| z)2xEE7s!S0sU1xQ7uY=$sfJK8-2oZfB(S3biKyWl@9ILNtv^X2#gE`WT~+=dHMGLm zMWoqu^qVf$MNzXxL~vz`7ky3gEa(+f2|)^@(`Y4vc5%re0Un}n?KG%>LECyqE+)hb z_iN3L#egKeDruKzlob#)Nj5mJR(glkTpbSQl{7ewJjs}U7`zaDQ(!C8F+r7K|+L4-2nhLt;`29YOc`)IYP?f6J6Se$! zArz%r*Mr5>n5% zBHd^#j6&;uwZmKUO0ud*5fP3~HX29b8-+^5&C8w1jj^sEK%hTgEo6DVD(CMyck`2h z4$Q!O9-bq)hv}bug*{I5d*g+4ALk{YdN}WmcAE7iEDdJvDqrR0PC)KJEBSJ14{Ohx z7{`R&>Se$%4!QNS*^xE94gE2&`-TfXE9a^NoQIVwtu@#3J?{roLk8agcM#?d@?{?$ zwtBx38-+WPRs;e_&YhX{b3e1ohn!HVoF|X39wJ!h`y=_w!Vq0_De6epR7?M`Ty)C- z4c9@m`9$sX7yEEIiuUOTi?Irc4^Qt3;F6>2^ z18mzOjFY0!T{%%x&mi6Vqrd&#s%N@#xN>+}yT_M!*E^)p14P6d^DVH_?%-S16gR1V zu!yOn7+yu9l6xxB=XGuWfAqKSow2q4%>wv;w74VnwVp*1hxD%!W#T%?9m@x%P$ZUJ z=G>&oz0VD8FpoZ1ny*PyjAVO(hwDfF6@{w^7NhnTj&$iBpv7&A|KD0%!QS!VHy5u9 z%%R8MH*FgU^`oU#`C*;Ms92$|5}D1m8(NR&{x?Vu_A7)Zk2hHIC7V{P}k zi=X7?E#$5X!=;klBCFM-t9(j?w4cx+fsjh=yQg$SO!79C&)^xm>9<=Ca9XxS@`PNw z3NP#-XK0iTY~;30cE&upiN@Q_W1yAU{NO8wDR$!sf@)g@jj=p9I0WbEm%vB*UP#?kEaTK!?+S1;SY-UNFwVs>`}GfI2#sTO&UFA@6)sD7 z&i3ui760VYTSTR^vph};kL!cr(vz(z^|QRC5AKuY?+R?pHorHcX8<2=55zPMk)t8x zlYn%f_e;8tfj79Pwu}e0G2P*iORE^<(3b|)cFhQ6_Te^Gn>m*!5v3pf;(Zp|&=NrK zxwY#Kw=_YoEK$Mre8lFHrz8^Rt^FM;{DKwjO2t8#80_yir(X=^BboqDQs}W_01us9 zOr(D-G1_RF``IR4z57|LKQg+O%a$nm8mFNB&-bN6!Th^*D*HqN7N6(&Vi;9A7*X|M zy9Usj&0jr0Yys7WkUDy?IiXi3a(@PN8Ol zx^WI8K+y7sI3Ju3;WFMD$K`K=e;5rSIKRNr`K`Kq6NrX?jDh&@2vBMe>a);z+ew7% zz{BoD)E!Y0wVVlbMoSJgT4r}`8IB{m=6_YL4S3hKN?bVkr_V;cM+#Nt2IO;6Q65ZJ z908wBzk~MRY-=uA)G%|vv-YtUsd~LKu4&r5YvPwDaNO|@r@h1*Q$_k&ODWyG(GhI0 z%SG7@E+ftyp^W{$x(7(pC*@DeFWLsWoco-jz)-mMo8nY#%l7KR4tLZ=mCMohH+(rU zrFM7_eE42gv1c6Z*X+8!L^-3-b0X%@3=2=1#U()_Ylldc$QsvzU=IQWX1qYCa7@fg z`<0n@uKnTRSUrWzksfYsCY?sjR|DBT!?3s!%Dvm4;lF(u-GArVcv^WON=C8 z&g-z!s#W`{(Elw`bv^u^&1uTrTYR+t)n{|d7oy;@)B9Rmu~WlzXGlwO-nh~)@3=tA z3Jz_HQ3IdgtlhUmfSb}#S0DZ!>kpc`KP0?D+P?|hQ)1b~RvD_QV|jr0D%o8B--l2K ziSi#RCTubGtofx*Vd|6520cUdwl01+aE6G~@KN<>L`oC|Z56Ff#?*cmG``#->;~-; z2!8aw#1!Y-68@kkwnIk7g0~^h5H)b`J%)3$9_k$Y`RgDrtcE3;OyN`mA^nMZ5zoQk zhFoNW0`#^Yxpz#a?NRz@XBs&K2;Ag2Ul-^NBJV}0Uu?bpDR|9jdgd=_^s65^x4t0@ zBxYMR*N6t79DY0O>ooZAQbX|(`t|D1B=s}+D-Mf!78rI!@Pvxw?LnHPf4f`3cQEq7 zYzF-~T~2GmN}7`)id%5=)kh&gfa;VDdm8TQy5B0?7?F$mQ-#02qP@oZ=rSXb?qGMy zRxUh6u#fd?3P%*Y&`p`yK!Z&7p;{ILtB`8{J%VOnjBxJV0>|8dzEli{k>$bhpVa^e zm)Z z8Fi<_TmWV2fH)lLDIl#(;s3K!Wdb(!O3)21xs_|Vj%armq>Txy1+O@JF5?n|hE6(GO>W zxfn#CLU6_4=six1sZ zbK@ILTBt_YqbSheo;nHY?%eML#6npFgC7VR4}#8yU)$Q0RQCK36lGD<(Z{rVqwICY zfT>1)=W(_hBO9l```o4@U(=r$XSVyetzC)eUI|4hhNeLX87Kid0gQZS(#>|iA& zt0HK?@x~J9bu%%f+^5PcP#Zit>uYh-QRLN8Z>@4K5!o#r=|TF{8`6CoO6)k11Sb@P z<3#%Ml;M>A@l*lOAP&6r6iWPHup^6fCNwH7n#kjmNU;0M48yKH_pJV(g;wFW-9mJt zlHoKl>PmY(F=K)R9DRAz!+i*qLJ!l+M9PDn+f-N{WXh|rx#i)bZ&P2^#wQBSEBJe1 zrY$E+x~O#Ea_7zG;0;oxq_Z{9y6zf_>Hx{8?OF5py#cvP!ynAj41t%s^Vlq8EE_o-Ou#v$nP(NU8ZejYA+83YH^y( zafCd8UYsAmJX;BJeNone>?TN_T)tsruGj`jdbQX>CTo|0i#k5Y4ny<`MwQu|g30FH z&fXm&0gVBePY}_s++{nMx>U&5jB##mc3O4l4nutQ1Zy(&>H)Z3VU3Y9zra*;511aW z18L}qHM07NiK#w&cdslzOfU)Le8Ik_-Rfl}>`jl&1zh&6F(sT_Sr^7(zl$-BDsmCD;ARg$>K^z>)LqirL6oMPUuNTH~aBX&;Sc{Io!nphk7VAp} zn!T_nJTqvy&zm&WCTe~3$)0uX$+*~Zqom8*I!U$IGZS3N1Y!M_*F1injb-U`@!-)H z7t|H*HXrqfyNU12M5bohf&%yCh}bK*M8W)#{jOA|fpd|IgoV~*#!ASb8AD|uuLlqw zeq<*-skkl&jxzOG1&hbZrk=8d>4&qZTQdv$dMNF z4w-wjEp&fJM22y{Roi}*`((xOZ48bD>by6l>%lhkqgiMyh~)sNMA`7t68kFGxLltR zfIUHAnz|w3(YLQ7`FPW17AjK&He7vSoB>$SuC+^gzeZ!K$-A3@xzelgci$DCl+~lB zCBq1YKVR1jzK|PICQf0*hzl02^8H4n6x?4(7G?Oq;=)5=abSMd_{Ec$@lZ*Lc_R*8 zC`EbTPadN=+$HpL|Fh(0uYc+aeau2VbDtW<9d+HK)t z{ru4vbf3Nw9we6#;Zh*@AXZeO2?r#jA@6bv^CEOc(*0e#o4|QK)2RK^8qHDa=3kvG zcy!wc;2}h6zh)uS`yHc;z0Ge})>Wplx4qz3q20Vn`9R-L?5U|`2cgV$yR6$#u2(;q zf77H)6X}cFLA4lU2}d-iVCF@XzvdhG<6y!v-|_f$kMMKzjm=E0?as8ykhZEOwBPi* zn4XOvHX9(yyc*1ZUy5=6CLq=#E#$Meh&Y%jk=z=GLaFwF`}ikJ&x9?o@2g|q{^-XH zke5XQd0zL=Cw-H~k4^oX%M(+uWt0Mm%uf(T0Q8w!g^joFov0t~ zDh@EfO_8C6qmqzj%C(TB{dDzMeDKzL1)561j&IkpiT zVCH>lPfSegexPkc1@D6<(gpIfq9`!%|yccF1p0Hvl|5Qa6a1D(8vlg zbUzqT*Z-*voZc}{)nkE1Rncr9g$1S-MY4Sey%?98^9FJ@m|IpNF@Ep6&zO#5XdM6z ze$3h1+h5iIH0d|cYO?EMs)5(u02M4v0X~o9fhwAuGR#m7sXct&(H!#@zp4HK{39+| zqPJVZjTSFjd!Ib0gxX3NLeFxBJSkpW&DJhi_VZFV=n^bCjm^r;?e!)F&_{SnPI&)O zN!H@}dWQ$XO+RP$3`TW!z}2bMW%8&()q92GOSMVDF*nO{I{O1~gWVuXsvw>cYs~Lq5aUfG;;d$4Io20UY zcSIEU0CJWD!umOhC0YO^4N^jo_T9k5UoY8ep6O15vZRx@C)joce}L%8inl0;A{F5; zI6v>0p7t#LB+-75R+f#k_D?+$NU9jzS;$AMC9^3@S8q{;EJifL-2^MOA>hs&J|vOOnypBB?~eypX+n`@4z0ao0OR8T(kA{NMsf;uOQLsS~E9* z0lb85SO7Tls~?pbl#Uhx+9C7eSwm|0S{x!^Ff$O9yIh@G`GnL+CRO}FgGA;jd-T?} zhb_vdo6yb5O58Fje~xh+&)*SB0fsi)VZGc-y+K{o-qv(~7PMtN5Da-kp5{%;vz|av zL&&AW@|D*%~>7{&pK4=Qag(o7y#eHa33#Q5b?-Q?;ie+Wbo1WhV9Oku1$k&*12ua5#t zJh0lXNI-#>^qRic$%qo&Dg#JJ2xe<|0_3>A$S;36#|WP69d8W%&Q*Qtwq4Q*)MmTT z%EB^XFdI-hj=#iBEZPaiuZTP-{i@w2GdVajb z_z)zf!F2MlP&d9QfN8Tq*I9uA+dEY9&BD?$lN;YElMiUGU{sNMogzTIX(_Juwf1(w zy#iommcu!zV`31^01ePQDAJ4bt$Ob+lSk=NN(W@W+<{g!1GM4B?+wh_^&!FYCKYC|eB1rXmXdwxmP5lnPpv-c8#Q zk;+)6v<(Qoda-pEZ)Njn_aLR?tfVm*)4cwX0LbL~vr9JVgLkf1d}{j}+_s$_($JiL zf-n!H$tW8v_X&Jc887i|DvaB%0|SxXPLyU6+~A@$IPYjldn*&D1`4o8fbfqESCHinm>dW_ zyt)0{@CO+s2o7woCu{HHbK6E+_|I-iXc zc4%UBtg#CPBM3HGY;t&?`;n4%v6jj3Q;dx>%`R!FREL-6b)EiYK=|M-PF*c-yn&jB zxYl_m60Cd5z2PPN>AG>w$3h_`R{gv8xH~O~Pc}{yDGB}XyyA04n-@OjIa!a6@yhc}2V8%2AM?$V{i2qNI1WDg{z)Qh<~Ye4259SyogSfXo#op}SY zn#=o3oM$nJ24y>C+fOPlW*twfJFSVqhcFthw{o8T<8Ko|h>(Z-QSY`aBPTa>KNdlk zg-UibDVw^knMIZu{}4NsAY%k;Y0errM8}CkAj-AA>|#WLydaNoC0D|SzuH;cMAz8! zHRUFmSpCsj-~|Q;?u@(Q8?;s;=C(D9;>e0uAuWyZr8TV2jS53A^jf*nci7B21poYS zTAV&4AlELUIdZg~uI5+*FZbB(>aS_T(;Xv(hevE?Eq}l9mpGglBrwl3FYc`z`7s8y zYWb(Db+-EB^tIax4~64;f&c<_=SJ4TN%MTGV{39#UE@a|#SN2w-r$rjlL|irsy1dfx>Lo_!sB-+28W@8AA}oehDk(OI$ArH?W93_KZ|Jw5$@ z#x@Oz2)k(Mr>l-w%0vmgJjKxh@v5AZ2!oosX$(a8x;TRhu5V+$?8E&`UQ`AWo{$!3 zsXoXhBf_15wxu3sRC)6xoFZ{;&JhZw_Ab3F=be+ZPt9=%1|_@gY~2wo z2Ll#m2d1Spgz6Hz6&A0Icw4{bKduMAcyK;dGtl?#lA+0w2#EK;73~VpAYdwC&3Z@a ztcLG9(huBjGOcDv?1TOZ_~)ce*0gmU=I|27iazA=@W_-8q;FE|p3R3M&F+_XS^>%Z z5m|u1uYJJ)?|;p`;20GKN6^FqJCOQ?VW-dZ}X+7`)c+=@1vR?c5*kaJk6 zMrhHz5<=&D!ca_)+i!I$WTw4q`fFlgN0R(wum0 zoYPLaOZESJI6rzgcvOTK+%u{}k)~%6aS@bek89E0H&8Ekp)-g34Mg#rU`eB@L+)Ms ze9*wV=upjUt=KpcEF5VaH|VU7?BO{o-h|yr`4VEwruJt%0)H-mhBV-+!0*P3OH5<} z0;TlNwoAe7S^wRsz&JQ0QmCmbr6>|-+T%)K@#`=WH81}q4DG(3doX0#W#^;_8N`x| zsG|?`pn>oOGHupv4)|V0sJJ7{Bm&}tAm{A)Y)_W11)>lk>gi((xm1#!W7T3{Kx9F_ zlJSfx?)&#O7Dp-{d99_T)v*gwU=XS~hFzRhaScp_Y!Ux1br1}Ah?LZHCs!oF%$sM~ z#BdP#R}*aQOf3_tzCGS-nqoN+13L&KslP+cCDEpPuldK14V~r?>ehn<|1&m%D$F38 z*po6u`N2XveV1}HBf*rCb>>J0IZ~-{D~aoW=|Ye$iw5=|X(qLNS8sTorJB}uJDPpj z(x3l5CWZ|2j1CknBwe*YOqBokU zX?q0$Hd1XtgA|r|%z03X%_pA2p@H5Wcv}ZG|DfX0Y;&XB-j{~Jeglpq(zh9s89g>% zbbPNenopNM3;$cnKZ}4R1bh%)`;DATSnyb{D^LB@h+5~Xo#*Q?eX53BVT z`6zJ$(mYLAmj(SDL<8C3%g2ES&_TpSR1>DIMC<@N*MWbiHMbi6(xUgpA;Q<&~7Xor3#ew5uSHq~OfeE2}V!`H*4Gxqm zQ^6fP5E}s?7pd5jHcsPA+)cGq!I0nB+$Zbb*+4cK`mcq@@t=?)rAVc~ zpW@$b4;~K(dX7%W8r{QMEAIeSex+d8shl#Ah(?4%xP$e1?VlaWKn?+?;xi7FZCy%e z@gpX9?2~jd3q%Qlyn56yjtE}{1RPUCuJ~ufn`&L|r+2wfgH2tdrnPhJaiW8^ zRQ%y!#Vgp6YLVuHkx1X98V0FrF1}j?`ykw&dtC=&QJ;A{aiD~d)`Bm>f42$*@P;*k zEG`ysFvJU@z5{-=rkLKy|JEAtpHg|amUz|jAOjRpidcbh*sA+MR6|MQ$h1Z*8(%5w z;CE0l5mWfPY_ok+Bw$QKq#|r7WJ_~Ga_X~E>AT|cvT~=tB2{Rq&JzpG#G18jHnZ3i zHwK-jQOp|Ie&hu5cpZtyw|L(uIeb{*R1XlUre^_CD3$)3goz@J2 z%PVUf7>tw0Yeu&j|D1k~0TcfiiPu_uJ*WxAzHi(WDYvS4ril`vF2AqWPb8+#AuSwE zVo{@7VZQ-Ank5!@G{EM&@gzp0H%*s#G%tb3^LHqw``*$_qWoXhJ)%mb|9fk3<=}Yu z0i_;83J#(J;}uqM&unJ}jQCssxqAIg;j0ii7dW=K3G@fdq74`7PI=I;!fqVw-ofGe zLV}EeslseP7fXwq`IcUu&FkKCJ+!rkHxBjab0Lw8ApV)NO18=G6 zpd3^~ii>O9bINr``{^c~bPE=`$DT*JXy+<(35h#$X5Es}bwv&5&r#8ot_um0xe=YZ z0+7uk41zygyWTm{hf1~h{LiCGX$hee6PDJ}O01lC3&5S>_aS8V3L?6;QggMPQ9Cry z-t4~Dl^msTf9?+@uJ3aTX18LSlc_V+Y2Gy2M41wH8A2wp;P!3I44GQCUO}7CCTtUm z5W#SMznMD1zLqj*Y3J3$y_d8YKMbd0g6iM$11w_j-EiwYOa1STq>J=_sJhCisJ1pt zi~^!kiUQKzB_K5*ARP+Q4I&`Y-7O$3Eh*jIjdV*lLnGZ?-^RH2erwH|#SezXoU_l~ z@B6$@0P!a+Foz3&1{|2XD(64p0aeQKz=+^+8#(;vGln9_N6OYqw{P!W{{9Ft5ghT4 z_{tqA^ydhAHpU_XKco~eNDwNcm=MV@LAI(hAsswn$KJI^+C%Jc?+%k$)7j)r=c1P4 z&!=_;I->y?H-c7G$`Ag)YQJ`1U*tG$TWO%Hr2>9!A(f#0R|SdLSjpM{_iJwI$o!W> z0(0J?8l>zb0#8ZEf=Fq`M|7&W5R_1S6^aHKu5`EoVXmCf=D4@w-sz-?xZkFG;Sck4 zV=tTgkCF(4CMz}u`}SPEUvw#{Kky}-bE=*iOIO444nEli#o+YH>hFi)&m-}Z$ZRbw zYCR~9*HPgg^UXov*8TG*zTakpsj~h2lJm6T=1RuQ zGLp*NMZBnB^)?gzHfUFTYa>4lXlutah8{k9L1d&*5P5iLl?6+32X)a{#(~Rt<(pe< z>=r`=N^pskDU&dOf^zriy{kvO^GB$j?B<4XGT;0lbkxpDmDDG&5xEIdV=mxfkOyjF zAOoo5jBadv$8AJ!sb|VRqfdNtL>&(ackU>TzPPqv-eLAV4fuW%MesM7;`Rhw!s_Vg z2st}gO$_eg@Q3M8K4}WP^vByIcm(`=N2%wXrfYV(IE=>bmR8Ry8Kmb|!xQLlyp5jHW#<3;3**AW#H zIy`)Xv46QX{?kqqO}pPy2Cxur*TOM?b$wjW=s9i`438WkL|+>&`nV^*-YsX=bh@sx zayfkSKqnKkhEnD$85+-NlB7cDs>E>79--6R<1ZNp*M4%7)OC?_i}-XSJJj`5xQ!DS}n5_+~22wh+ld4j&)-^K5hW#WvFTK$)QNtOv*dE}%|I~$-@nYkC++*!#a{f44YJmS`FFvufIaww#wW&1~<#m4V5nX2Mg{n!t za%6!Lv3Ix>TJnu2`HJe)NR1-n+DJ{sqqoLd|FpZ5V9-iz^7W?sU>vr=IdRR!yk8jfQ)3 z%`FF?{9E__y5CCwMG_ys`S_9aUITDU>;&KE2%#g`c8%oDy+%X~5jC(V%2ORlA6*ZmZnWz9S9;b6dyRm6U zd^(OP$<>uJXMKIDATjpY^k0~@NDA)zyW5F8DkgF`x*f(HT9W^vP@jM0gmzl z{!<6EZ_kqLiNdhw6&Xj4St~Wob}F&zr8IJ+9&U4t^Jy~yG4$-DzlC;|r$sU8LhNyq zI1<6d>|N5@GXM79vfAIrZgMQpTUC1V509~P3(o{^o0Qo0L2%hWQpHA?HGCMMv0pqI zTc#}IHMz9gQcUE~gvMlkv{_)N8#c#1j)4G$$`fJl9_Z<`BNt;L*F)Dc{xlZx+V+p` zi|xS@?b7kUnBTPv%swC&o-wzIc-lzDD0@_FyQ3Af_uNW<3+VB9I3ac93}HC$#$X6D zR_LH_{wP9Lv7S4>gakp2<{Q{#M(ZV_JH|m?=tlmgeoviO8D8!J@4iEE=UyJ=swK`I z{=jKvVhPu&m&Ff4))Y4du4}bBD}Ph*Kq)s)?xqD|b=bN2-SHLpcE?+>xOsgloPb|O zs{4KctVBP!CyrUr_Bz`O(plhj(H|k>0;K#$^|vOs*Zr7#4`CNzm+u4vtfPghj-{by3XwmgW6cy|N~>y|EB4b{A+mQD4Y&fTyaBuBOrb^T^BbD&pBm2|k_`)NA3By~cmn8D}xQ`FU zD`E?QIWHpAdDt_-!?I2+#LVP$|EJ-deI%8_JhqRkOoHGe_rR#{#2s@d@5%T36z|$) z+r!;04AXSex{d$*(O+e)CNh6XyWj@kT>+1HMSI|QLA`Pz!1(|<JH04#u^1+Ch4rz+ZYr{TRN%;%Hs+de>Rg=}o#!EJa#GZ!gQ^ zte~Ye9^{*uaG7hMi-&yYC0U#APNY?2aNch6L|XRCSGSG@&2=Uz>E>= zaP~~;(xw_WL^StJb2H8=S|D%?z5DQKlCga$Ec~nC?gR+e08(_G1U%Q}h+F^Jb~p%2 ze>9r}Q#U-d^AJUrdQJK4%I!$u_bUd3N(mhvx@jP?5SlsYwKxlHlX}2}w(vq>)ZChr z0dV=S#&CBR)q$tqW={uVW1jugun;dM*lps4)5b?(m$t(X@G5`NPqmjpF!Y3#2&1R` ze2C#cO9^a=JP7D}PpEuCKxruUkbL_+kw7(&7flNB?N>KrUkEnRtSr_7^xf!X4RDBN z@PF9?C0Uh-q+8pstcg$P^Spt2IZnH|E9Jm*JYN?KcdgWq0;r*_h}qw$vp7n)Z`c^8 z_2-d-kJ#Q^jF*4D5FRF1y}{c8)Zeq^R&)~p7o4fD330~qGwv$^$w*S60@TNoiG{o>$QyX~YwbSAwI2yC zape;V?OY*+qvvbHc3i$@yzlQYKvCKbyJKRWfa8=D5yZRwvfShEr~p86-fqfpPjC%_ z0`WNBH=@;n=4@b)@309mQQw76 z&cWc&BVoBZW=kl%LUf;!f5(+P${dY2Fs;V1=(t1UTFC8o(UChc*9{j|#`~V1B+Fj* zwXtiiwZ$(_h(G-X(J^vQBA;f>mzH>LTZUr?!e-utbe`=yXdH&X1uK^HdPiCz;V?nA zbo5oflZ;|~`=8Iv@xUWjiPj7M+~|?MiOw`7g;PyAF5`D(8kw&!UALt1VDuDiyk8zo zvpb2~6m+Fj%C6?gU=Un%um_$=2ZHS!pjKg1Ut;}Fu@8E9Z@732pX?ND5vvzvCm{)?rggjsHSf*4%@%ZmwxxI8m&mrhuXtwxUF<{d33o8dXz0Ws| zs#zOZlF7cCQ6=fb zz}@@U^D2)1Z6X?rDZ!VA&9;`>RsXlJQXvd=hl2+RXE&Dz!Lv-sE~2VUU4OpTuU1~? zt6R|$D|(zJCAl|4at(hzf`wopUh3rX$WliUX|Z#^&Dr}a4u=|nH4p_RX`h9m+5CKj zy&r7W2jkebMo;Z|sQr=h#tPgXg)|)BkdHclt4;X%y{j~$cx_c`u-@MBRv891^r}sq z{FCfb`SY)oZIt2}?D1Am+cR}+%%pgm>V#XY;Qw$MIC!P4mODw$u>Uf8es8GC6m!xa zKOx73L`N@MAX+Z~Yqg~e&GL@O8K$W#n(iu*Qr!GMg-9!W3BcLkCQ_J)BCgd%qz1982U-F zgq{I?XV0xLxO5?QdG*zt-wp_Y{03N7(8Z7neF#Y>9 zVjvW%qsW)q5QPYgiwHcbc#c889h>Ql+=A|tBcFvmn0W=?*=bS}2>_ib-KH1leH(_5 z^L7He>AV-gJSFmEP?BT0yAM(CdVGvB%-D$8>MQ(>aQtbyKVrGrfN=pDeoQbEK+cG? z0{3rC?I_UX(2>)F4KsN}g7Nt%_)M>qrSZ8R7y5q%XKvP;W*34g_zfb|f39c;Cj4sV z72*eY`SCr2jY!lt7f1O8s|sp;vS~cjSnybZ!7m>vBJc+3Qg~zR@xI#Br-}YWX8t|l z1@Db_Aw&z!I!G_iRC@H^)<4J>M~A)(Lw=(|WS;X^52t&u-pBNG2vmvvK% zXlW8z*G*nUX4}c_u(@3tZ@;o_anZ*CQ^Jck7rRUS_NNB*H-euFcz}P#02B_XA%vPC zbbkYAsog-NQf(=rv;RoF{J0M{pe{UWB>PtLvlAEQduo<}_(b&49+ty%rN1ZbukKC& z-A%6^TZ)0xnKJi~jPRAlU98Y($K8d0)S^E5hRZ z(D2s&MXlaS?89g_xuDMj%T39^^%1?naOy97t}g9XH6!)r?UoBmD4g0QlMPO1#kuM= z3hXB1L;y_+d*2%?l4A*e+MkKz`~qx&4PoCr-}S{!L~K*{Mo`DZ+ZDHoOnS5)Ec=k7 zrgjcVEzdAGAKD$SooUpzv9cp`CKP>Z{hOoxtNdZcs5D>v1i5oZ44vh>P$ID&x*J}6 zOK}L=yW~gvi1vr$Hh)(vFZ|_;*$UG{hG=?K_Oqi3*t_ZRqGFp~+=>V`<55W#{jMeU znXg)pk1^1ia2D!|Ci_DuQa+wqd#Z6czbVeXP;I@uRVi%%xrw{B_WpqkpIBI1;Bxyb zf2*T}!Q2xW>DBGry}Za%z0Xk*X*a4Aa?lH5+nKRN9ISpq*VC8aY@^8vfBd+el|n`G zPo(Cr)xIm#_|XkD<&r3Rt$Fn!Ngl;T0*w|f3YjeeFyjcl_n3z4#H85u_s&E%rwG|~ zv;r|22<}{hpYHE3$;pTTqK7q}zOay}{ZWN|)8!dv99zzF#mEi7{=MIDRb=6)l=9zvK`v5+xL#fsF;476<1LEFc zhyeUZyA)9IAuf0MWjw#&#k!arTSSpwqzI0c$R_g6sqzlT%yKAlT}F9EOl+JTY`Xmysj2)I7R-8QmWc6wD-m&HLoBQC)%GfS7S zRWgZuEqo->xP9>!SSrPRbfR^>(;tFChFmNtH+#!=0-1!*b==gT&SDe+*RsJ!0XRdc z#_dtoJk|DVajlo47u4DcIC$@X>4Jr3=f%=)Xfq42U|t)} z3d4qgfdysCaI?cmw*2w{5M-gvg7}0{VDu=TLj$rUh9WJCD)IkKct4GXQw!rcnYJ7( zSLbg3%3XV&U1|i=;IiJPBNr5S7jXUtmDr`wg709X+FCYM6k~0)t_HYTzMU(%Txj(- z85&F#BZUFyreeJfgd|-!2i+I%4Y_45JvLC3k)1{P&!`9r+J_Q{|p0w~Re>xx%NmfH@bNb?R_hTozv1 zuSI`UDtXy<|3Z=}0bg_dXVju0A*zh-3U#jr+P|?qpv;lI3z`j5{u;FJcciJ7G6I@p z0yKc-pnSFM4mGU;-H^m$UiY;5&8=T$y5x=VZ zzCdla)lkp~X~qg*KXk4s6Yr07pX4Yw3ke5bzm1rp^St}uP+m?6?QbE?{|veIvWs@I z7(d?>_qjomx8sIzLE9Yyr%8Qz1{1ATr#Hs3{&2L8B@2&8t{J8TJD9!{^Q-cS6=sM&w*=Q?X;WiG7_litIX0820d_otYUZJ^uThZqY|LOgC#6 z^lN4`DEv4QJ0}5zgyelT(%3!=2$}@pj;HLflA(~Rpuu0V199>uNR(yVBbF#@m za;wTxdn)E^Fzb6E(cZ7Y5<$;6)y{b0wGe#)l@6rm$b@%;WDz`9O&|o!BxTii$!T?Z zW^U{uU)di5g5%bPqIBxrn@RtC2{H_KT(%D#Hss^q)qC4Lk-DX=p#V-&G?J!;9V4IS znF&|TcgE00rOKMvvQRY~$^k8+M>r}j7J7yA6nlkXp@t7F;jlN1FvDcTxN8yF)2jhE zXOBa4i9A#6ThnXhW`P-LXDF%2(pe28< zhnQXypj2vPe8A*o$X|@%ReWe)HHElfsWpXOXXhfzU9{kr%bNfEW0ok4QSjEzdP87eG-iwVaBK8wbG8!`h zdTp#NN1MZ+%!5U`83rfY(^dYp$#hF|JC7JX5}lLY$9+UmPuBzyzii2IkXP_IS!JN_WD%5VahE{gDzam(udk`(uc-I~c zdJ0P~y9Fc zgG9tWZQ{IZU3tu6(xo&}r7xnxHn0aZ$LXK8w~&YtPrDO~(lAyR{BfpC{`|hZOz^;X za6`xuT3+zu_E+JS)2zgY}ihwUjHi6^+wib&ed=Dy5aj60sWeIG?EM z?GLbHxNQ^ihvZ+!KfNg?bmeKm&Fcrrayge1igMewES=*;i{Z;R7Gtr8JUX(w-Z<)|3)0l?%fTEPWNr+lBKL1J*Q)Vb>$Wq3nqJr*nwLey*?1x;3*PH+0Iyl~RIL zvpcn<4${(B}j)31>RH z{osIvDde;*av2xyFKXsU)6*~MBF^Z#UJ);sM~1D-XG?S@lP^xzzKHPOveOBhnm~QO z=I7U^t{K(AJGH#;-EHx%ALDGU7ki~eazMysxvfmWVI$o9ErJp#z;vQ!)0Z1wla3^b%P8sqLT8w0My7dxxj zhe+yLT6GEq8i@36c>WgvjHlYT5|hnQkcvmsxwZseP(X@T-BtIaDnb&M`HkiiDzjja z!T$E5#8&S$rT-JgQS1H~R_?D7&x{B%EGvIDeX`C`EEw$MKHd!fmiQoSGpocf6iSvF zZn0A$Wg6s<;#2QzYGQ(9ROCaDoe6PI8K7W#-C1rN3O$J57=GXO57;(M<_4o6P7|*( z<0ooJ_i|F|RTYHS6KhbbI`i~>pm}&2Yl4P;wkOB|XnMzFyzq13p8_qk?RWBw8m4HZ zTaYWibY_Dn_~Q>~+k*-P*RN#Lq{zD!=))E%gN{KxoEugZEW*nX60jg8NdDw*S8u{W1pbxl*odTh|iNVmDT3n;I+; zufO~((rx4fcueS%DdxwzY=T;F{->_g+ZdSL#0Rt_sTpr#}Pz3(G=mSK?Wb_<%24H5C#|`J#T&c%gIOp_AA{ zcGr&W4J;q0UuP*4eHDuT{N*L-UD2t?tyH^E%ygrm_HeF*Af~7Pw>9K~`Hhc0zCldZ z54ls0jF80K_G#nQ+rH(9pyXp4|D8$6%;n9g=7c{NuRd(^5mMRTCJQ4oX#!DS< z5E_mFRvQUcgCXsCbQh6xyvmoL2=ehbj&fr4YAabV2_47Q{UeY7lLa!O-6l79cx~Q9 zh(T?=UjDPH(pT3;d75D^yyqx%hs2w+Sh=_zl z0K&-et>9cW@JF?^k@FH?J+YRTvu+XE6CSfxUAp7NbzRuh-u-%7f5~`WppH@=29p1}C(9bTTKqI$Npz+uJbl*__vZWggU{H6 z*w|Oqz&_mK@Moxjz1=@}R>&#j`}gn9OeaBPV!NI2dE_lzQ1~+6yp`2*TN208BA|Q; zEe|t$sIJaPa*@-Zg`e}A+IlpckrU8S{03wGyz2wHJ4R`=snhlkE*Or2xbM-Dd7S<@Dqn?0w;ng8)yb5j@925Lj&~iY&-DwkH2=s*O2Nco{Kcn~v#0yg|J)Ps zgS!Y^HLu!|;P9Ah$pS!91$^6DTP?OKi{YeHECeqVm?Z8 zi4%kfyGcB8EL1V)FoH?ujvd$5N~<9}tw}$MtGH90 zszOAwz4*A=?w_OZclAUxne*`eep$bhZfG#7iAU@uj9^#ygS7-60fDo!<;}G-A_2l> zUcw7&5{)Z2KG%EadjcKbe}D5LA}t|v(ocBivot-i&q>2m97rQo7YnNlWsw|Ri4aH| zDY?(#F~(JCcT%@ecU&z=kPzzk3eb$47e68CGz^gi#^ZT81@L-1apP$Gb}9U`I^DpH z-P{}0fl4nF+h?w-GL0mnDAA|qZatSs&NHVzi}P?1CvZOg75nQX2$x)rF&__cP@*P% z$~^0Pa81W(7DB*N zA|w9ih3(Kp^(kiIz2jYpjYl4=NZhY#0uLzooKmoRR9}0rw&ris#gtUm74SAq2AxT) z)?5}&9>f@BlqUslgZ#M1)8xzvzg7a#9ZWOKby8bcDSyT1L*XP25-?h@G_jV`=}OWt zVhV$!*egk@hUW}^d!leE?`_gP{P;J*V_*RGl$R5CQ&unNT5U_ z_qyfCS*s$m0`*7%`V$p8Bpk=#%$jIuQ7?04=#YkOx86zsB z>2P4Txo-Dlx+OM6%%{x8AmVjhCryjm=Q9*eE)-RcHtb{`1yz1upXgvFhq<2yJGL@; z{P=c+bG)AY&t@sx=&@!uR#;cE9{$U>zFmF%7NobOIQI3&0TpjF$x`s5`cfciSM(;A<)8J|OAd6MP7f9JM>p7>F6+ z>o8*LXw*kcq;+DPck1b6K6hU3OAF5O5&7>O#S}GxTwh<9qI~DO|7Yt8iH7KqZnk8o z`&lQRmWJx-QfLX8l3K(Xu~yAjD+myJ@Fou7@6NhW1*N!5s5K@9VDw^bIKMleAJ#C6 zTT5i?bJ&*R{S;pocQhS~f^WLLn3Xqo?KMYq+Y>?a_>0z6eWswBtC8e-7OYbDGz0 zpP~U{HqWEhubF_{{6Qw64sTE!C3_zj>Yba*=&Gann~=`6A0^O5SwXZSOks1_CW;2m ziB`G`8{Cv5b}HV^hla-<%Ins0R~`B77(0bno2H z9jZ5jh-Qgm=$sg5Fi6738Ax#QJtHW=?Yzi4>%vQRAF_*kY*gG;38j5m($But|<`(Jem?kKoKMg z5we$o{6M3$ees?J&P2Y4GjD@8R5HO3l%peyrnUuPJdWKOXISt`n-#_reao6KrHy7@ znX=Ep_XctP+ewHLp(8hr1kJ*6pyPJ4{~4CKHDDFqJ9xnn`O~1HUcnr>jRt>Jc+SQ` z)6+^fE67r}Dd->-S=ys+4fWjpSb1jtM>UjMxRY-OhnYh_8130sVxYfZ&IjJ>$^bk* zjqSP$$ zl@I7fq0JGd2kBBQMx%69X7e7mIrCrH+RKYMnq!7C^Z-$-*l~=3`gLK_EmZgg49dcu znTh1!5uA=`7THt<#PTR;!LjI{f5hQkkPFgp45Y!6{1@$^_JIQ(-Ri~zvw59%rJotk zAe)93#7uwCA=JssYG+moG_CZsiuvKd z)kqACrI!JjmF2@QRhCLwHqZIb6bs<_q+Ir)Oh{N*+`zy(>uuH-#;=WB;PPWe^E-Ll z`%VdGipT|0?d(K$ro$YUYPp>ZYBEt>o9)lz+236?PyeWaOdhoFYvrhHUq>_T}+vt z6T=ByWrmWbfe;SNR0`a5$pkv7Shk@Y>%FNo;iL8a=&z-XMvEX6+F^voaQARXx&o#0 z1@cAPgZ{2E+NF!0gQYO@dD)Z?fw6lg4lR3Dh@S@jd2qlFDB^IFygVC35Z=8OEZGVZ z^(+7fc+7@Jgz*cYS2`SJV-7oZeSX}XnpBL7dat@_9D-)}CL=jjxg`AywcPten8RRd zXTHw?kct{=H@yBPFqr%b6<|2|UOs3M8)K#h%zQkTSb)fyvpH2E1E6(5X4q;<(>*bw z_XxsC>TLZP1QlWzJ2Ma>S^Jp0At-*X^6kRCryw7xY5$}{cT8~j((|!9k zyhqM;EEo(_B4`vxw^(o(;`kVW$U5Sk+(}M`i%7G08?F{7>&Kt3V@{98@(pCRyxbn5 zo;UkEG?X|M&NltofD+AfYSQ-%KI3AiK@)Z3R9zdbu%$nYGB}(;PFOhLDe+G2MSkhW z3E2iPG^g>$W(cPoHoYlIbY&oPzAPHfd?Tnh0A$;W@=8*#(?2TXxo8}f&|RL1P%Ea_@?@*+K4O&pJ~9&yO<+Lh`7gUb)C%D}qMGmd zOLRfg>B`WbfpZV!u-$!qW4aif>|^*`-QV@%I!y^4AFi<`^Tu&}7=Fzc=ZQ(s_!qx+ zgiB;ID&LfqXkcb76Ib@1KiVwmUq?FBZNCoB%v`i^t-lyPazz`>s3-0$5BtEpr3?kk5r#ARp;O<5_5iYr zj)oxp-SMNh{WK`(g>!|~lI|TC)W|XA)|M4#Xzd7vn(q-at>iEd7Krc_WS@dA`rQvV z_(37Rt+EKWn>frQ;!-p@50(~ZFNSz-iIEQ@5I&u5nj)M9=THXqRMoNk$vgL3u0S46 zN7r)qa!HY{bxh$)&v%m?FwySS#3)0#j4mDW_rZxwFEhMpnJ);IX(&M(%<{G>%RL$@ zo0l)xXnJ3%Sg-}{!Gj25;DmW=+Mg#jnY(kI6kWR9+Bm_T_a&w2&ekY#^4C?V!M>cmD@igpf7ygYTlpD8;_(B|Wsd>=m5Lz96knHAP>oVY_ZHF9o5~ z>9=E6m}b={cfE50Jzmomr{TRuL^J~_I7raPMPb4NEtTYnZEgd4dHJzjd>Cg;nayq4$v z$%Fu#U1>D$LY~j9na>%_rqt2q08*|ns;3&R8f#xIt@zSfHc1Qq>MNH^Hk(RfuzaM0 ze2&5p!p$GRev9z9Jivrx*9`g#uV)8%J;z94v z>UxzbmzgxZNz~XykbFe3yn3~eCc%|2*B+!o229@lpDaHqTSwb=d{2#fya)32D5VpI zBN?LJy5uT(<0a&YKW+E&sM!UMziX)i-l!X$PDb^uUB&o(m4u4dD_c!j@n#s1d8nim z&#lYuB(5eNeWtS=I?>=*w)&Jt#0GA@wN~8jemA4hdr>A@=jCF?Sz3DSRK-j$87T$F!Y~`hE5y$54-4`T{cO(#~33PSFgW- zropmmPE2eO2X=zk{a=KnCEm?mdO)YX=zWl*HJum&GgcvV6N02KADd-7m~Sg7mScq5 zvNc{LNNK$}PCbcu+7;0NLi^aHnA>&pH>=*Y?{IWavA*k$lviFtlaI7|a7%bWFmg6U zR6DG6eBrth)3!hB=zRF8X8c){I|L!Z4P#+OxjRL3lxJnycUM7?je>!*rg-n@g=a>E ziFt0W^UOm~loD{GcWi_ zG?;+5N3S$Rjyv>QL2<5W{`8&idPEBM8g&RnAh}FDsZMJ(sAimpfs-;xz~b4#8=h`q zK7kS_S{&Gtm6`1%^CZ1;h>ZAgDd?GEc2g`^hlTT4+;S2{Tbt{B+{)rORy&P(ifrN7 zd*bV~%59qDGKGl2)nDt4X8mWp4Il!y9TfhW_757B*P}X67X#A4%emt#dSy}+(Ndpy zP|s0SaC4?;7xBgEE=~sEuu^ew%Y~Q4x#X;0`udwb<=95eQYuPz`j)!~is`K*N7s%J zvkJ7;U+Def)bmJ6_(TjQ5^kF3*wbyY1m>a~_L5u+fpBQw#?@k9`0i6Y6#3kVE0TM| zJRN92Sj9caCae|MD_|br{}gwltq>07G@Ab-d)?7;-fC|wWiTz_{szdy-QT^8AN z#FOGnJ$m{8Ku&G)P~kPR{N>8fDRINYaF0Pdjag(z0BK%r#=-FmH<>4*SAo9WuQ~tN9ybFhs9}>>`QXP*;?;>#8LG^hn%-rpB<9o zbLo$y>@#!^R{Cj9pk>3JJKY0cfB*&2nD_}cd&gr~j-u&SPPr)z>QJ0y3Ufh7EOWRk zbvCbF`cs6t-loD>uKV2d?95ueG3=2D9NsGu{!2E5!iv%TDvOmtJz{j#yftgG8?i?S z?|UNiRkC-GBpfLR2(YNocX7Z8i0PJi*`Xdj9fi+2JZH4 znzd!pzRl?4TL`C|%TWY-;~&SU8{4nNmh+L4Ij(~o?-jT2tYZ2Q6fNVmn@u8Z=w>D{;&kI4hq*(1dt(;!{iRnOl4|K>s#!E$6>ohPia z<+326>;WRm=@&Qk9?dF?s&=1`GAA}CaNGGkLA=~$d_xu7CpJ;!3VU`h3e+zETE;rK zH_xLGF~K6&Qvr|SCP6x_k~cR~Rq)cb_VIG)lkc@flkB61zWTdp2u~dYC|krKH3oz# zoNwC!QU2+v%ssLnxz4OHduIVkN7GRYj7~60y-JJa&k400!br#EPnr3$rVLFLKNes~ z^vHaWHpd6cHKtB0vV71-f&lsA`Wa7P246+2G3h_}w|lq{oDNjs3DP|LJ#6TGvXG~c zY2~l2R;D$4SJC?yT=)`$6(1*-EQtxXTGDL_Cw=-=KDCf)LF}goXv*>4%%;Ysum#J1 z6tM@Jwwmh9*xc7-9x8~!F4?3mkKpH7HEW0>iL?LlgWSHNGO`eU$U^m69^=;ocMC$S zG1$}k%7!e&(ftIvm8-P)^tx+@!zaK0X~U`F_~V=goh^&Cfr8lMlAWF8`i6SKafbf_ zy~`>hTef(qwayUjDki6GrVxGs%%@Cqoa_D+`v1z_zLb*k|7v z)qixTD>8_@MWy$6N|&@FC0G63SJ}fTtMcRI zGmLN30rSfghg&e1RF~^Oo@&MQ&iCMcgtU16am}--lSk84$V+zDA`g_%qw!&3=_1e7`LzVnX+1kmRy z?$=kmd}PAiaRH2T{d{Tn&V>Vf(1W$|1D_JoOQXC(zI9&Q@~!y1MUmja)4}8h_sX#z zO_WF*MGF=T#$s(}6x!O_L3)ly>&s93J6ed*$IShVs#@0M7htUn>%jnXvY=_kLca2& zUbj@kbltT74|u+N2Yz$U+wAJx8pXp8ssWY*I^^`n9KL)`l4h@7d1rvy@D^joABXt41z6ddxT5v5(jvLxGe%G-7B#g_3tOL zmFNqpePBtxsvo*Y)iQfoX7zR|_Gk87e=M;fan&Lf~`%~m-pHG7NXkshRW5`v{KjveOpR#@XK5?Lb^if`5mh{_XkTcyh zy}FFABQC~W>-JZ^BAk>tb}duhb~OgUJom}apCAaQwmXi-8w8&aQhfS31ZD72{sgI| zWCGLNrfUrI2cv$jACqFw@_WupjG)bNErGNXrxBJXst{Ne^68aZQiJD+Z4qa?*L?&E z+l2)iu_w~RH_;7xw}gwOSOS6>{c& z1^uTx95fOA;No~UaD(Td0#{VcKgiYQDazoreUoP z!<<(itMGEjFxsHu2{Q>geb{yyJt7nBJ0Ja&YZjlGeOw4PZjYN2N~>O5Lu&aCl?8O? z5KMPq@JYilW54`R@6P4UK#E8j5bwATyOgu>qs+a$lkCz`>uW&5On`t9M^`2FwGX|S z?>)I&%}1%&wwXl6b+6V2-tomg4c~X7s{>$VjWDns<8Rl!?;gfkGR0k6r$-MQ1xjG4bJ-4P2 zU&8rscCROCV2a=KFb?9m(kPEj{CeNZKirSmK_@-uj$pHw{CeinR5tXkjBO^IS;SG@ z*Ye{ zpHWNV5@{$4u3JAFbZ5oVeJvp^>s37Z!Cd+9r)=aMc)b-pF5FC7O6hA1pe!_by{t2n<_=Xq57mCW`h zz?PWXPtDk^ccTj-yY-@*@_8szq@&XRg5G54iU_wtrrYFm*+Q+CRk~eKc-QI;E16!d z8jSfzMbA)9AGk-;9bBB81meLpteNrP3J~)V>?#cdS1t5yAxB>+tL8ZjLM$;NEo3XF znv}zhfqwEA7s>NBp%XMexcz2@cgsJIC3u+_L5DO*vjrt9@*yla56m=|+|5iUweDl& z=DYQr#Lj%d)yoTuq9EK3F8|6Na}i@hK8&%*?l5PtHP|XCOg3DnWO|7*$E*MNNHE8% zdX{ysAfD|OV*2}n+;ayyO|g2Xv!x&qKxW9n{Q&nla(H=3JlR|%9D6XmJWY!+7uGdt zq2tL;X~zv3QvU)!90FnshJX3n*seFx(}GZ5e|n zkcI3*s8$@??WW2_zqzg8=Y%JqF2H+WY!Q<@^?&P&()M`-+1O7{`Kwy{9hf#47?O`| z&x{sy7*Cz5Tj51C`tGL5*E`G4taWFQtFK#qfl}3d!hl_wP>AM%Vrx; zXQ_P4W;-iC@b!(&cQlcxe2jsOy&}$XRk_#y07d*US(K@o%@KTz(*)X>R2`4`di$Bt z>T=f+7_I0s6dgBr)5OsGA!X}cbt`0HG`@*t=W2_ z4EG6vbk5oLRZXrtDouNR#(pZ9!dgDeWKB`~Krv7Z zVzzIk1pxI>{v|FD4*ZfWpCexOVPvVD@JSa)L?*LdA08~6AKIVuMB6{u@?;i$C~t?k zm6vZ$GBfyOmDB4RlCKV}xCL&TQmfg_ZWo=MTWth<21AD#A#yWdf5gK?*r@;dbOc*V8nK^gpW3IxMQ~@8Xn%NOulMNtb{~ z3P^{5gmiZ!-3roz(jg$C)X)swog&>`(%toL?)`c1eg9ORN0~YMoW0L?t@T-6Hm8Cz zJ)D?NnnGtZuL}U#x9gXOz<<6i34~uL@At6~(P*U*zziJ^brO3cTR!P=Z$c5ET8lWl zz?|3Fdgv6?SPK(awbjFKb!+ekTh93A&gL%_Z^MOK%v!r`Evoz2IVJ~k0T&TQi&r1FgL!!hJ&EgEF z1l8uaGSL78PXt3yf915;@{$p>Vkwqs59`)@zJ$?z%O_Vt2!})RLMnr2EsPu$qMQOZ z&*f=`c4`x;?pc8lps0-G>T0v;*U<7GFAvL~aL?m`dH83VMye`tyZ4`8&@H^Vy9L|o)wmDFIujCzaz8?yi zv$v?*l4B$=pfRxdVMTOw;~1gZlh{_7j(@T&7yTb&XVV?3s9#`ELvKIk8cRVSq zj{_!f+)qX^hZ=kYHfE}00s~`TKfX@1VEaz#z5wq5z`r_iY0n(RY~S+zH4)d|_|NX5l03x~;i7(2z`c4lM22*R$+V zqwB1Kf;z~L&i5-k$)4u07u5|ZlJeQnN_`}+X1&>-be6TbcqM#g(`RcLu@6DuL_4*} zo%D}aKfUF)L!10;Gg;bQ)ObU`JmS^~D$j6N)Uh`?NGw{wD_vZE;nvz`DF`l{_O|_J zAZ=Q}3iTWH_T3nOGgSrHW-KgaeBh0-s$BD5EjG%SXs(L)@h!H4C?UA$It|x{a+U9d zCA?QIHnP*@InyB}xjR*in|n~)1HS9J7d7=mt>$OvM*|`!^r{?~(tWSQ6+X+Wd_V<} zhqG>5p_6Y0Y3~kqTI?zImf4NBC!R@mNH@uh1dI5bkDCA2`+~}X(DBkYnja0iRtBS8 zpn#_l6D3B+{0VUFVu?@om~3>06GH&e_4hEqYgf-G>DIX`hwlA7%&yRcl$rMCu^a}= z)_eYUgl&+CVH6@HcqDJ^u~#qb*{W;v(HdiaXCC^-*<<57??=-D50A8GNK{j;7>dB> z?9qoF`(uE2JwLPuJ6EHzA%6r=F;~rD@TW=K=H;T-uj@Sv zpQ4_FD0mEa&tap=YEPlNGcExE-91?f3|YB#LcYtyNHTcjIiEt`+-H(tVU}M_dXv>E zSAPoF*N}3b3FJ@RCndY%`%_joK4A3kT;HAp05jv6nSQNKW*nQYR>Q?wie{x3a8sy& zvY(yZel21@eJWRRiy*39rNx#Y^urc(_9thKe2qmBY7MYR(27??xG{@%sZI?cyl%iW zbJiZ2u00Y!NZ{-|rHJXPRPVT00^nrKIG`;Zg3`jP%;!a07E{nSnJ0*LuEjm*%uU;4 z7okEOyYvlp$ej@%k)WBkLuzv0fq(FXz8e38!jbdyzuVOHN4SW6Lx%@G9H_fu7IAxS z#%;(KA#gfsR0~4Br`sf}t_(G5Cg50+Ig3U%Zq}FhB9uopmM%@NDsvczXr9AvhDfXL z0#u5}7Tr$iX|G|C;IavMKGP=2yvbHduPd-T_isEmVy0V(i=C_YnvMS%5=aB&Anlt8 z?b#)Vqsc(B;1#_il%D$Tw{$&~-nSkbv-t^H3gwEIvr{?T*UM3>lys#=xX!;B(z~!^ zQz86Tv64f%#!|q0aZ=;5C3{t2Yu|cob#*#atW2|NWRsVMowz@$@jNQ~`Mmcs2BS_@ zv7ibp&-**l25>jj%pkb<-nhJJG<-jz5rZ+1=Ob7A?n!i;_uE^H(MGU5tT7vI*YD$r z%#A;k(s#G=e6-RXcT8bDe0&ZJryXito%355R_zEq=3UV*ln|Qx$y@qDP#KHVANF0? zRirk$>iyIcX5O*0wX0RGAMj1QdrvsG_xfr=Q(QhqRuK;bKTZ;+_RKzQzltH*P!ph6 zN`tn$*v~i49UI>VY@zf`P_N4j$n>ZXXFzn~P1F3hBxiHkZc4g>-^U)_9mLFvO%N}- zr?{b-bNK23HFC*(Y=1M37$;O|4L1wD!Be3QRa_+pTDRQ z6%FK9>ghO`qg{|gIEr0m0&{dp7}z!=28rHEOQX@PnGX^E02I()3j7WW{!S$^Ycqlk89W!9aYcT;SFrX zNM7XM6eZADws8zbLr_d>4Wg`#eIVh}9L#7{ba2Yf$+-SpURDWjRD!y;WFyyweCcT&yN7vp)eYkQvx}ou*kd z+bgZd74O?50v7I?Q6Lr?f_CURfdPmpJuEO;1iQuJOt@WTP?NgTioEA@bs8sYMeu;v zw-X70*RL}V4mBrA=rqyJ4RmmfSe3_Fsh%*r^|;344r>;;a>B00esOt#i2E#Q+S6zbq48H!9_0taA>U&Q>Ca zrqRdEC`7#Qm-GQg<-x`%$B$>_mDE3c7q|mLxwkjUoGzR$wQ-VR3N+lx{99Q-q6OgDHnP#d^f%&ll&b9KRu%cW3fFmNEi|$G2&sS14qO;>VKi z=ZnFkbBwm->0VXo0TZ_%aq%$l5*X4fDoJQvdq9T-c%7lPg2bi7anICrJrk$uKWZ&<)HC3bkBMHCt6R^sYhu~C2tqgDW#zM ztJ?X{g6OErg|!WS9{v$^1v5=Voy}OU^3+%~MrKF@pJ;)s04rTwcrIt9Z)dWP^@o`y z+n$c6xhr-?k79JcJa>!#CKYAH?2+0@U$qsS`>i#<=pp^Ut0IwOM896D-vZTBoVFha zh|N_Bql$a58^fvAypoHZlFxwwFDfhR@~4+<^;XN(v7eO@?w)dl5)BXIyE8X{p*mh) z1I-%T3TvFm2>2()aBU|4F1LMd1osBCcLYSxa$;#Jeq3U~XY!>_IML)zs0%fL>_Jhc zSM!s4Y;x>s(H~DT#Q9TP0*7tE_bl2ZdVNoY__yQ-8SRN zxU6T3g?ZtP7?FIGT$1Z@?Jvgo#-Fv|5+RUHh3%=(aA*Ulm&d_UJ52lWFvGA}6{ak( zj8q^5e-}dFt%`h=Papr}Hc=B+1ib2vAPhGggM+Ap_W7<;+1HZ(?YA*X!ns$Yp!rzn z6LySR1DpF6B;?%PP)$q+5xT#K@Gu6HD?E)hPv-d)InCDvY=#r9^ajgi+q8 zHS*<#KCGH^Bob6$YJbYPR;Xydv~%O{5H*mGX2#W@e!18^`q9!2Y*FeL=Q>pGe=UKQ zGbwmj4GR3c566(G|CX3H;k@^eLbDfPJ|2mYl7FV}c2Wt_ze6QjUic#$3M4u9YiwS^ z%g|ZuxhN#uWKkgUF1Lg^sbK^^T$M3zH+w9Ti1RGny9Ik&0zlc^3j*gaQqaP~RoK1w7b7!XU>$cXaE^ zdn9*Z-T-ey<9_gMzP9k5GG=@Sw!LAJF7R3eq&s-|rzC2@tgXYHXHXjUg+?ve?dw07 zUesHO!7~S@d90Cy7DMq%*N)Dd&#yn^*kHYhD{@AXuQK3z0IzNdbFePg$;IF`1UZ)-acwnO1RD2EoEt{ce2R($@r`3QM2R5>%;H}NI?UY4l|eiPd%at-JwnX@DL0Aj({l!P4A^RfDoS(WJ@5o zBnO~?r8G3PY@98abP;r`z({D@{t77YntHHMc zT&A>2b9JTi7h{sYbD^*FUllR|8fF&j9K&2`-Ov2Prp?;a(#;P%-tV*1KaQpT9r5MB z_*zw-H%z%um|gB<3ZFl|*c0_!Ox|T`t5^?9#g1c&QEp3EcE(ro;zWsTVV-ZSf&Ytf zrb%BS=M?+-D;_js43J)o|7eA{1hXCoCgL1sG~+rz&Ku`ZEXG2RaI=XBGa$Hg3EY9m z(>Jv=fMNSoCdAGe`<0TryL-83U)H;VhdyYP&n8eZz?jquHfhzZ%K<`M7e6rbyR3D# zu7TX)h?MOnYGdGlcWn-iZ&X3DrM0dRi*fq*D)t%&N>>%)1iM~ zzaPn;t+-IgMmu;jAQogDr0EP*d8NpH+bFksOZIfUk}wqg2u__P;w?`lJoPmpt1ss* zSqUXIox;fUI`y8TtH_+c{00(*FIqokHi$$ZO#f#}cZb8#7<`cOET?=*aA4PWr4cGf z6VUV+?hJX;PXiSzitWeoo>KanKce4lz~kTIsZtybTaK6-&n9xPEG40x%IG-wTLB@| zBsj!5NM3c&uRn0Pqn;Q(Qa{3;F1WJt&Vshis|kO@K~e70B3$Bt_8D&2>%+?T;;+!F zI+`UiOMZ3u2#r>EK9*=5y4b!rD*Vy%+^gttHEPuFC^pWMs%9zxb^{I!xmSTQ(ZHTs zeQ7QdP)lkfi0-TTz!1ZM9iefz_y_TKUhp(oqW29~1Ge>qparZg&nk?76E5gli3ck^ z+RTIVF{va_|F&_4#9RN;{nbtY9J7aJXe%yck4T=r3LQJG#F1HNKT9E9oonukvKTNU z_7*OGknyvcu`j&mWThTM3*_h_4y@3xxSZ}(EQh>OJ0TBSSoJ%^s`MNg+=Y>*5A zEbKwPvcbV^gp(=KxYGW>$9g*O{H#tP=01B5JJVTBuaCezX?7}1-rlEgnk9f9lrf)a zXtLrxxj`+!Wx8p(dbyPeo)lM#-k@r$FcjRm>WX;!?)I%WR4t$aO=qXoi!8nSYPVJ? zo@tr3D%EJ80^FDl#@)&PW(=r=LEded1Trw@mSZ}3*K$X9t-uv6nt|uvXi#REUe7N5 zEfMmi2dwY7Drb3g(Lq(*y`GR0?Da@y@{kkMU7f|I8p>U`{pjw$Zs^OCEjo$N58ZST z%7jY}0*FM!`_q_ioJu1FTbp_-6}Cp+!L^SOuaV+guoog-N?-yLWmLRIG`wg*qB{yC zn(M6)VS4gy_#OUSUWp1NjtLN5q|8z87h>>5GVcu{*!~t#4!@yV zOO!esT9p;HwT!q@;VVI+{?gJfFtGg8ODM)dKzp1-s!@&!*s~WRpV+eo-zd~!IP@;lu6z?jdRv>ZojXV4M={u~2ueH2 z+q|oW4@_L_S&l!ArwyRKM^9jlB~^nA`yLdW)CyDjk%H?IR zk8w-pbM&U4QFpx;hKpcLb1Hcu~pcOv}GX9<1p+daNFL3J!L5Jv<= zMTB0AfWR~3@`7;R#%)A68NAkJCt!{LD~0;Aqlz|mmd#RFO`bFsM{La2RXkCq~ni zQ>6MB(fHIMGMH{N>Ir}GmH$)Q@z;+#BqxH0mR*cOFD#It=ZhT-KjN?{$@<#daWZls z?A|Q@3FYYF_@_{WT)bEd=p(OZ>IWrqR7eJIa#)W?KRaHYT-*POkd7i=&eluU_8wiH z(i~Z~2=s)yHu38&_+ta3P?|pu67aDz1eFndbVUpa1`dm8Brw&=_RyV zV?@*Qk-c+Qsmk<|l_B%f_kYi?ta!@A|F8h={{}Y(E<&S^85PT9?In<$g9FZFA0ZSI z4$MBU+-gP`$NpBJz7jh4v8^uH)eSNIm{-oz*f6SM;VZhGJ$6L8XR4P|zW-YgPY(#B zGax{q&UjOVcls0h1<9^P4iV)#@|d^j9i)XqLLVvQ3>EtrJK`!5fC;MUgIwP2q_1#= znu0g93lUnkjv3rormoRp(d3BMJ)}A;4p28N`E=F zI)o&F&>aWYW*dS&hT(YN0RNR7p;3(r1<|;o6!Mf^&| zR-ee3wvpKDwigE9!4ukX{{ikx3xf5?ul_#Y)825R61vjU7vV9ermH4r@8NML8T2I8 zJxi8(fM>3*nph|JS=0t~!N;U>WPE(Htoc6&INAk(gmIh8#VgVWRcsgzv*zXx|7=I1 zMtJ@f$Z~p$EcCn=v2G%o{hb{NJaY-y9}BLk7f8j*aAK*a7H2}5IyFGpCn-;+^=i5* zEuj7ObU(#wDl1T#!=`T#a%s~+Xj`Jll<3G~Y%^s;A>ylX(Ta*JNw$B>nJL`h*XI`P zQp(KX*`oe0{Zn)an}h8TNLZ{dJc)E*cx;a&t9_TAzIXd4+TK&Yf7HU-o6J#7zfF_1p$8>@}J)hPgpXRr)vYPM3L>rSmb{<0u?BDpiKd~cJI1tlUoSIz5oLDz+*S58s2a1UYJYCIIE7B ze9X^0nG3%vo;OB?Qu2#)M(s}>>&l0Y16pxEB0buK(CMB3*4MYB5GTpkxqkn z=6DD5zXK0xy}x==fTto(V5~_0?vFjFJ3n~)PnQW22Yi?jyr8@9$lJOosv+wHM%gO9 zcS6ai0hHs|{e3Xu&}9+qMXGDB(hj%08j$u<94xE?)rvJgJ(fg}Krt(J0jC+c{k60B zJCzOF>df!XjamX^H+e=M?!`GvoOrZr%ZrXi7(<`fc>h z|Ae0aQCb5e&T^r$%K^{GXAg}snhY2mPOsnazuk^ra6g|g^l9^OSTkTu%+4_BPJP*f z&q{M5wpAVWQ~$X>CDTo`4!_^aPDVs54J{i*hxCiyHM!i(?^zCWElY)IAWPB(WcBJ4vofq!9k zz)g=1|N9j6*TD&L)wAaCKaC*LR2yjSxNWj=nfeD=kgr+R&7rCcu}v;wuGbCX4!&aP zd$FlNy9<5XP6z0Dvd5mU-*l~yIHqYmci%SfI66s7D{V{szzfyd4;Q?Xm-F$suM<^IRd6D=xB1y}TZ?fdL@#&C##3$V!l?>rOz7Ktik@KSi=oMH4!LCcLI-RHgY zn;)=xoL#vY{l{}lwJ`fc;*bFoxSk@AmH%*xUa9Ui&ChZugmzI$VDQ&?r-~Zx#r*b( z;q15fnGIwH5&u9cq?EuwGT`A0-n+-@fY33Hh(>n|Go)D9!HO~)Z>MnYp!CEb3T#8nfFw3D|CT+ zARN?~e8wfU+rYjR zCEEj25jIGdGre}q^Ad~gN<04Mun2Ai|1FdjL*rl8XD}g2;{wOWI zSls!?ABr2{?^yDU^~jM(bKMzn25GrJ?0-L_n#8-8d0}yII(hD7im9(svJ16XKYjXy zORA;Iq?U(Y@Y^0J&^S2UtAOtoe@}h`kzAL{wSH}|nD_y$lo9wXJ__ER8-M1tow6|f z!Ul4HEY{!008eLvo#yl9J|J@Ikp*lB25yP3GRgyt$m^3;T+jsvW*@j7z;v8CmS0* z1+(7Z0)v$}aIt&AdE|8?Wc&aG!$V4hhl9JzvSkOoU#=82-iD0@1I!=uk9irOR%VF@ zpAJGd6sEolu**#o>4mzo$E>|()9NsBbzEqIuLg~|5%tM}mY;1@-dWlNdB2A4Ii6TW zoJmJKx*Z|ZKjq`>7);o4$=d=c{Z_qFteDKvbHkoDf*Vp$* zxuHK8V;JOy&H9g>sT)$PUl*}O6@G1tn5_IpI)vlE~a+@ec-`k&*Z0; znV}b;OIM@il5Z$?c#F$^?yS_H8B4b9R(bpeM8H-+2vfS+2@ADtw_7$a85HG2oZSGW z!u0pAud+~CI*{y+q+i=p8OsKan@?i}+A_djwQ@FpRc{8_dCtd!%h!R-Z!%HqR&?KNZ@{Xfixvtxlg9C$@x;MdwnS_UpCJAG zgYEnfNov`9l4KRpqme^}zBZ7s041JY8$>z=fLbd~ZvES9;3&AheK}F@Re4=!5}L~I zEV-kmESr|6F)$+w`Ieb!GB;E0bYEiry1LXZq1$2c+M}7+6oke#8gFgwm`e^_5QaqjYQ3*^5jjc8k zVexPx)*-?%1C7OI6y5gb4Jp&4NVFh&E2aAwMJ-NL7`XUiRcHj*fKsia`X4&U2q@8g5c&Dx9)g_$7eS(sOxoFzY5 z%B2%0<^DqkPb0Z8ZQ|w4M|-!CUH2!OS6d+^y8pc8I#T~ma?jcPKpZeF&BXisD18DRE=7XCKFej;l7N7N*CNv5 zHs2pF>@%m@;$=-|q4etpXl;pN-y|_iFAhxyCA?MX1-*01Ee@b{cfNiuHq?9dXXykt z<(9+W)l!JQ%U^J*e6@N|U9{~plxIywlA-f+``jnE5W#nsAu=(CFVGhH^WgBUh_k+% zR7fWty?m@$f2MXq!^v36P_;8d|7tIL=T<0Ygq%FQk9(=3uI1~>uaAiLzgBGPSK57H z^@cl`hkq!Jo^gaO%JxZ+glvh&rF>Q_YP9=|geQoVi}nr@_l%UBoJCytQo6XN zHE^!pv-|6N6qo8WAHSDeK-yCthA>=x!R4@ zd_%qH49{Yjl1SX8k`SGBoB)BhBm^CU09qm1lqJ>QNjYo_p3`5n`f&}za=Vk#vkH@m zo31RG=`&-0**9QyM3FJD=L`@@c#VWU#S?_JuQp@#;NK2vN%bxndZ@Z#k0h zyTf9O3T%_c>hS_|fOR}5dhJLA;pjwH?)T3@cK$w2um0}PUH7N1Hq|-kDbqraKBow9 zrXk#X=Xi~RVCrl7GSdIH<)O#bsjbQndCMK-me=T?&_%53zzbH-GYV=^v0s$W95Pn= zQ#2^^FA|~CHG1a#uc^vxCd!oA`f8svlazHebk!ej<-rD_W?wcVlynBt>~qk<-C))b znyKW=U-Q5`wkeb5b)P@?qwk+B0rCIRjRmuf8)^2{DT_GVcX#o?F}t~qy<38S0Kd!E z6USVn+lpVs(3o29sz|C~#X@Ii+hsje%+#XGpViJI_(Njj9-V!av{$6_zKP2tsx&Pa zR*i-wxbI)V%R9a{u{f&n{R?ih0Aq8biF8PWvJsySCk7^ZjQ;34V#E4kE#+7@_Ub{y zr4HH@s==f13#%x>$23xrLym|jkD@OzNUI8!8+ppzclj-6e?*R`j$N}i9)ISMNe>PV z?hVevv~PvlH^wQ&Voyse$Tl6`!kfOa#UO|y%Tu<>L|CLY^}zZ{xWvLzP-2x4p8Tsg z*lk3XMANLYdbGj(^(^rn`1~7CcdVvM(wl+&nKv(vgY8cer;Hk-Zl2Rc(P&mJhdp&$ zw&=i0-@A|e%dG058%K0gRaw{MTu<4n;BoDWiSS&6#)+53o(R*sPz^0#afT;{a8c(! z=Zo<>Ppv;$OHq;b32stpblaO#$$M-0<)mw|b#TTZ=`i5!(@bHOBkHwb6_IGkDBFJ` z{SJ=}bXHK3@UrFa-NJvrXh}auKtz5Ni;S_^o1;wQLZjycd!Nq@5XL*lW!t+AWDmjl4sBy%75&&;l^XgU*QXI_oap0WdJLgl8g(C{4`-(Bw29@;X$ zM4F)8Mh(~NYPkmeYSCmqs~@b82FlyGy8V`LuJhUEDK)g28_ik$wo_lu)nEZC`Oi%Z z!q>;stzS5oNO0=Rps9!YHW$ z+5=(GDdzCV=U|AA0!7XzBn!1TusU72){B`ZJ9P#+yegpeKL>bO~UX*!ily?HVvkP$C>{3oeVqyb)-9~wRPqTNQADM47_6aTw_jC%{%)kt;@ zqzSS6$NyYlq7Ui)%}K5sI=LmWDJiiR``;TpH0vn*^V3W`e{GT~pORqlo@}m}-?1zt znK81lPYMfI4f{W0XyHU8zOHikIvpsPT}~ljR};Q5QKI^E(0R2-KbHL>d8$29Rr|Of zcOe~^Sh@g5i@7~4?pF*FIF5&}4UkoC4`&zQ<`w39&s}Wekyh+ z31t)7%i*)L*-n>_s{bZn5~StVG8@0ms(rhW*A9D+a@MQ3s$ciwefH>wD&~_-6n!@e zBd5JN`~1*HXN6&N?%%TYUUJA)J9-=sZuab56;4k~Ob3OAjqtsTP4x1Da4^E5>s1i~ zt8AvXfLA=ZpvS8QR0#8Mr($?KFK~qys$rixjXk}(0rbB{DaWVmUT@A3-5vt(;@IH1 zu9qa*SN>6}kFRC31~Q;m*-tnHq(e{WsW8Vb9G1&~1Eh1^EEt!I`tx!nr-13M&_Kx|Y*!>HewYtz-LAa~`dq!3}CB6RbuvGczz|HrVge;@UFg7=? zvl-Q2FtlCZxsFXgO@MJAyYZ~{wnklwWovf`V8#!Fo|{)OoLJ1vmvyqK6h*Te2B!Wz zqAY>GgE>0`!?WR#{U|SoZ6U=qOI@F64*R$xAFR8XVSKJpv(svv9xfI`SmJz_{Zwg3 zCa3(a%#9w7SA7rSHzZ+d9?e3%)%WU&wOH5l(^UqXxcYu|3&YQ>SuCobSzdC*Z)chi znMUH2(tb9DvZ~x%iOjcF*E`R8t^Z8)X;7N2-t$Q~N;`8ds`ValZWsU?aUYS6s{iZo zx|X7b*r^>8l||q0W>nue*zB-d4(Nz8nYiLOpZWVK0rPVB7!Ur}o42Pn#-r~s3<7VA z9%||Z;DsI4poAUUO_sJpB{@~iM_$9bg43wkXxM6R|0DSI)6wU@{ogz!Mw-a86)k>g zp#JFcL3nJ#&1KSyhg-1Q96iJ^!j*`;N$iB@X>7VhdEskg@ zCa(Pu$xD4AXijb(owLt(te+zp`k#>7~&-(afl@nDbiQY!}gO5hAsnC=S4+WZ}yE4Pq`U z)~rvp8gF@C)Ec!vv@$IDnM!8FBvBMbz?|H9suV-tU!Lx~Bi^_-8^!!xcyfI}3iMx! zq>v%wqq^fbgDV(tcp=gp@R&{o010pc$)Ay?uE~X!LBS;WFu>g5(BuktKUX*b-Y5#ss8^ozdVE}u23C9^)dwkf>9W|r zH%B?O3nm&(;gIz*v0NeZO1s&JRjf;XJ)iBPm2P9cB(D(IZ{~_ttxn9mzEM9&4&o~* zR-feGm(7NTM%=AZ`Sv1KnD~Q9C{@VrJCNsZpJ>&lQMQc`DRs7u*TmimcCSrHNi8a&f} z!43M&evmu1=zypfY)EtBdxNH_MWqeF<>TY@bAH&^*R|Gzzr!x~9^9F|df$>h9?AbK z=o+XsVrfrY|Iv-8Na3$3t4*gXZ10vjk;2?K|D6=Z`}?xVNq6yFk(=nd?#+p}U((gE zP2dX!WeWv9H?b7Hb@zg;b=h(~HSbGgbJ@~6dfjj~XH@Eap>k5kZdV$GPRK%=z^>oh zaRRg58Y?(#!~4<)CsKNRcfK4(*gIoz{oCi{L0jg?&n)fA-BSDcvkrQbjUj79j0TH_ zqo2=6o>~-lW4`FrROKV6)*uaw!Da*^-g{!G$z02|jM z7(r?BmkB2KWFGOKp zxk$+a|7*O&c%Cc2H?~?|AB8AF8e{hRhCBRQXjDr~ifA639C28T>8m-tWJ$S~*1q-?DVuOS9J5?=jAwZhff0Rz?#7Goi5Nkk?X`Kvt0!Y#`S1NnII3LnO**UxHK?3x(mYe7 z@+e|X*xcqJlfU+9+z9DmR|~!TT!pg*fm0h9h^1xPFe_6Qcc+yuLoCj-iYWD*$O>f$ z)zRh9YOfhw(=gTht6TT&0`>{QYV!e%0oHV>=mQE)p{Sg3|0Fzq7qigpCfB~UCef(M z@r3N0YUsXg=3IreMAreb;G?J`0GAa!;W-qG_)o?4DoAm$SDK$GH z%~h`c&}5$e&oJ*VBMq4L_S^4hyKYY$u>U~d8?lK@^v$5nMmNy^^W@{fbqKpSy@bA6 zXQLOhX!aN6dV53g+a{%9w>~uOS%E@LJs@wljl|G@@# zdh`|}ag^ir7RsaF2cL1SZPY8vbW!`T5sk}O8QZ+IxmzaB+H^XO5K1VEV$L5iBZvUr zhvM)T1R%`09XP_3Yp};H!hT7@fccUo@_E8n56v!{G}R0;@HA&WJ;$ zK54nnU`0qR&ZTzkU_F%dL?2<2##~H`+bK}$>Z#n`7Tz5<2)ZtL-#MWxI9F?4Cg65h zfx(SJW#dH(=RgU8^oozvbZGNBdzbIolXb;^ROl#g+yU1?`Jqmw)WKO|Y!xKEGxi+% zy--;Dio*P>j>!g%_J6M)$OcXcA^FV6n+b;%@PNob?q%!IKT@X-!vZ6>2&_54- zsf3LA#ycJX8v7kg6)bnj!^hwsOc^jf^!#X7!DqNxxyKB!lZ)2Qd6OUHZ$jE}O3#|~ zjO)dfMMec-b=ZGi9F>9Zi(R`yez#_e4NX*1xa?4MjOQ- zZQ%oNTI_kGSKDfLXQYOP4G##5Q0SYHvn`^A18$N!n zeW`M(M!Z&f2L3~tFyVgvh9>t_ujg(?s28VoiZ2>LbBpT<##w`eSHs=BKl5Z^@xb!p zQSf)^)5n9C-RqpBAMp>Ggwq)_%g|nck5wYvXD#d$0o}F^7HPkkOE&`jVb9YzVYaMvEfgi2m?B zFL&UlaG5gM1;P*2FPlHT1JX^{#Sa{gR(l!XrH7^9g^V-yq$e5j%N5{^aLV?EPp$nm zz$cAeEyG8hx15j;tDzsjTyk@h`82F#c@gQbGNFJ(AD zUHeDaWDJEn_OgzHz~Mj^kLP4$h*}%|cZY~t(fP|ab}fAxayC$+=|he~{al4VU-npt z3l+zG8-J?=6G6BQ4e6(K0`)`jrpl;(%J;A}wt*gFZ2Zk-T3Q+(oWh@!$y{@+C;-4A0?`HP5 zWZEVI@BQPkDg-evQCUh^B91Y?$h(adc^`QE{`7k z`__m7^cny)0^23U#%OKjjO(y$$E23^=4cIV ziai3^Zl+2(o?V+B;TaUkcY`U}>9CJ*PS19z*L{{=}HlEM&xMCWk)Te;d3h-K^N=ij1CKtogs zU?4xqlLS=eXC0?8PQk$t--(bdwAtX05L6OQo5V-N&t8FZ6f4YXEOyrGl=!aj-b~$z zNntV(f^#^Upf4*&p?U5-$~64F%1-Y0mf_X?ZwFujKaJYcJ}PY+wLh`ZhUuu4h#`c{ z+Raobi-iSu#r10FYzdw4aE^Zm$kzM#L}KjikOr=gNRe=Dt+`i#2v^>v|MeGL#{B)H7_p1`(7 z9Cw7*hn2nqu<5;|)eZb1-!03`Q~)n1DgT{)_2H_*sVgXh%L1W&ATD7fFW#Q4^?zpf z^9@)CAfmnJMIVlCj)G+M12iQJev(xYx?`L)G$6fqtRedr)Sn#o*(M`McY7 ztx9$-6*ir^CvkBUE!ruCh=MHMdGgQQHXaw8@VDD`9B&)~eNq+#=4ic_Bs_L6*1rS6 za7Zf&|JSUzsXX^xwWt8I)7~h-S7O%3`)wxu`k8N3JD&%7Aobe`k*u>L|op^ zFARFF;hPV9iBst1leL(YRLRrq4t#D4odVN$c7D%esfQOOnek4pu9$WN-FgzKQH}(m zLRE22T^IAJW{sBvOx4?H%8;_}9L`~xzD^$M6Ig$4Q4gdHb1e;uZ)sZdUt--Dw;jds z{NsA|zluM2cl3>EL1e7(3(nL$CErgvd6Byr=fhVpE3*%sK4-J$ZC6H2Yp0%z4`}Sf zXVUqdf7ra*iaVS0n}6%kw!g@A6{D0j3)}+RK+O-2V>ay>$u*l!RQ|R?3G3+w#d8rp zTdZ%sW}cm+;q_JAgRgaWP$aJJEB$`+%|Asr1V|FY3anQLS)1Zm-Q`#!vA{RbHBmXw zyS0=4;AovJpdK_$PN)q@?YPFX>D#qk&--2})Ck^`%k}wHm@7?ITPJ{i5B(a~-RUR@DJawS)E1DFi9NpTA^XHh{xIe8SO(b_sKv2_r`(pRC z;rdDQ`|nklg$4!>4yLS&k^(S8E*hqu3V2-{MROs~cTQI(2qi}zL}8*LqoAbJQ_qMT zZI_vmj4La?^zIAvPlSxy>eV!l-rx4oD@ZdBdb2ivy<0`kdUIp8c8q@*m~ff;05yy{4tN@eu6vZX}ibc6(UsS9*B7; z9EdM`lA`=XZC_{Tz1FJdH*2y68QJzQ*_^JryS-Xn)zB-!(mlKPbg=2x_LMQ5qo6#H zsS0brZjj1*(5v3RB8e;lXat1pY<|}V!J@J0Pd!TcDY+%1IrR&y)DjX!#w{|Q{JXqJ*PrIry?&? zEwG+x9e2#ZgX;ibMHdnIDGT*mAYw-(`YD?WK*wLiF=}bUE#~)=kvc(}oX1`SU3+ha zM%5q|4>m_&w1JYc{=N{W%Vc+D6b%mIfH(MjW&PxRB7a?hK2)mVXPjkw#x>q13k*`! zkQ*~eU-<#O&51M}Q3!}xjB`pJ{K6aO9CA*|a zoBiu)iitC-_dY!+kghlVTAf{ZP^0KYOtQ4S*DD?V?<(E*n^F3Iw7d+PS+Ktgdic{l za(qav`WC6QtIT3i9n_C{vHmpvvelEoY+gG0#i&7m=n=NW?yNP`pMx9zU^z;phv`ca z94F-V$c55p#N%NZ`LH7&`GO`qr%1V>I7uPm6ZDaWb)5QJmP?%(`IJPL0KRCNsk8eU zo#fwey~tnMl_jc|Pu`Ir7lBqNY5-%{qVx; zRKoN6kxT6(VfjTQj;i+UEBss9(yLI(rkEQ{^f(4m-?WhbO<7gQ;eTg4j0MD-+qg!< zD}eH0p{UR?!>g&L`ugg0!6GvYudt&{o~Wj9OyQk?P0n=1mzM%O;a)$&CgI1kLk%EM zaaZ5Q2A@@QSi*huy=k7}adp8~G^Z;41@Pb$*LGI1`glfdl{oeN9vlKgZn6sT*xN17 zMIDp4=_XH6xUzy$F_zgRvwpX4%;F`z1mS7f*b!Tpxw7kn>; zpcJ#noeqa80ypt}#c{uOImh}vn_^{mi+ri3|7qJ_3s4k+58Gcu7~#!@`iK%6&~iFJ@{el)=5lfW4k+Fz~yWV06g`|k(WiqL!+Ek6q*Qrxkb9sT9yCB-yB#sU{A zmwNhkYO4+CX|7i9BbRC7`_(OqkQ)xFCoH~6j&vdj9jT7vrKT2|b$_N?3qmGK5@kIM zV<-GRV#g5k@Qj&Gm7h0fed|AJT^F+`ZvBepj(d55sUmL&48(*S)@;v;0-sM3!MmE#WTBT}m3opAOBj+g!i#AUe9myt!w#YIJ;@H1%3on&`@>ox&Z6`@8ATjfpc z2;*2v*I_l7gPLW>%1^;u%v<9GbayIz@K_284`9@ z?6Fa#cM$R@Iq3Eip_PyLj#F{LJFVrX>QP>rWnj+2c~Z}wAM|#QS$E1LO5kc%n|tes z=m}MOIAPo-mIzB^c`;Pv|M>a}peVbyeY#VSkXRaNky5$@lu}VzkPwhYnq}z{N$Ca! zQ9(Kd7Laa`?gr^*Y5r%`_x)mi-+#v0VMcU!pXWU1-1l`~VSHu5^A$hCRqxJ^akF3| zIt+u0d$WMrvpL0rZs4Vp{PkYA7qD+(nh+Lm2io5?t_l%6Lg!bRvqc|GS}cyX6nlJ` zL=R9#Q3@}CBs@vtad0L`#CWO`xcBw>*LL*ey{o|^6_{Zgi1K@Ilk3?XbX2)u0{$+@ zmuq-9Fo8!FXW?{2CzHFwpQhUeFO~V~SPa)Q(bIFWo$p|@cy?JDlJLi@kV2^yYZj9;By{nHMxc4QhicM+GXXho$PsZw=)ANaRyAM~NnqK+}oNfihIAZ?pm!$nji@r-ZUFZQ{+$%o$N8|*r zqI_73K>3z(EWwkRw6)>X2?>}I6T+tx=HEswhM*TxN9c#Shx`jeueY(Pev7fp11_b-8jsswqcCNMOW^hb^f# zT-Mp|pYL?V7uLrEKE@s0wqOfm8ne_&=!HX4&Rbg^-HKR%$(v;qq=|J&1mfYAoImj< z5SVPsON{tc5dTHYb?ZIQ^wAhyQr5K`p{-NjF9P8Fw8kx00Rs*jlw=B#IRX0 zbVsPX@9S6C`Z`~|;6vRJiqDFbwk0;_E|Vj`GFp?*$ z3c-*buUz+9NKH0>XoWHLz<%`zdN5I$G}Q}B>mtF)SDMq`c}+bcDOO&TUvqnViqfx? zeSrG>CCQIL=>Q4(1$^6LLM#5IwR<7#h$+X&Tvc8xrbwE_s-IehoV-|Ka38-YA-?7@k;&;bYtMZ4|*HrB&7*N~21Amf$l zs}a;vHc|T(b%qV+!U;627a6Z!M>a}Ny3iGsqT3KU#eW$LzU8l%coc#W2xbS0G!~Pe zjw7r_^7)0{MI9Q|vwMB>qhBYyIZXX2uJ1fhWl}-$umdpUjgVX>{bF?8KBF)!mUd4>rlHkCW?07Rc3)JmW_mLoO`OrcsS0 z@2*ht>ax_-pZC_`hR%YBCDC=;Cl|7&746`F(=1-e_>ufZ|JgNTP9BXAQ=>J=t{J zXTBr<&B_z^^zE=o$O$~)qU!jgNn6BIpO(Od(8xBV$U*umqYtD8^t`b<@X4*cHX#hp zDUkFIYZ5Bd-m5N7*!Y=qKsX5)x1Xr5vH;=Gt@IldVY_mbue(88?ek4Pd)g|3+>>Fq|U(s0Bhf(oAg^8o}J*Gm1_dY(e2e`2K9^;ZF5##$rl zMj3v8U+-8H&m*JyMe-xPz>2%FQm8rNXl+9ZH;)Ts8b{4^W4WJqjs-Nfek#%}?#&QU z|Kz#rn>w8Ii>j=d+o5)J>!;`|oxA43vqJX6%g+5n5}kR0Txk9u5|_gf<(+2+%imPR z(063$wB^Z15IP$myR(g`<7;UV;}Uac9FNuZ=^v+0hCSRlD6eh*4tmjC2NUyNO^W~< zI~r*+2Sb(nTba>Z6pwLhO}S0Ao+pampj3Ox4A%V&8H65ZKh_UWY*~iu_pkkOg%zTBRs`%NFPAN;6s+4^Af;$V2T?p&;ArCxY>hMqX%!e}vAdFadSFNQS* zIxoIH>#$%#(VT!R15-nn%iEPUZWBn0`BG{Df?Z5fROK4Lm0#cVnOOM3XU2ZTh{}iY z-Fb(%odl*Cc>c1-gLsrcBSk(yZoB^43rY9?Kxcyuf4tGI;nqp}z>KpO6)GE|rF|^o z`Kr|mZX8H$u`!j?Mc9)y^7Qi+w7i0{N|Pp0X18QSe?99cj0AJ|#toR96b$o>5NJBx zi>t8=ZQkguGtZG9F;ghw*peT`APxwMFCegMM=UcDlYY{S(v$AsZ(LaWZ@$`a#h)K- zaT(?t*aX13ML7%0JML7uYR%NX*;{y^#k6w2T-=#hj4I@)NIo!gCr=d$TqXx6AB;+H z+wfbFzyi$zl9UL3qbp=ZO-*1C7nJzFlAXSjiBQHVS+fbTDjqmaU&GP z&=vPV{1$+RS;&2B-FHHd7tbzEt#rz-QVTz+Wz1IE6`LD&%_DghiCrf7KXq%H&NrD% z5S1Cd{6SO(%RLS^5`{Ksc%HUU?gubuVG4^HzMLjgcbiDtP-0NSU|4UGgCv~+cfx=> z14KUv(gAiSG=4Pl27iaOsK+1s(!|ov|NIT-n2Gca>T_>P36Ze@+1w;yo8-JgD)NAP z^#H#6@~Jds>fNHT;t2=ynl%_6uSnnvBzu;xSxzK5dU6d zl!6r;t(FL3ZUbBw*%78Pf}T-WggI9d;9T)UKRZsOy?AyQsbID{@P=1NLM4IUVn z%XPzTt`DGzrr2 zuG~iVtQ=>e;ylpUGwQ^4GxX4GH>z=GDx>R9gNZ8#T+I)mQ%)mVKBw=~?&KQham4uuh<$e+ z9a;alabH;~#{hDkCXPGC4PP1;JN}(RSxe@2x{PWWGdOfn#jEkjsf-RJ8yc5vFxU!P zjmWlaPnH#JKEov3s`-y_94d%P(P*ivP&DPKoQ6|{d+rNNj? zh56ch0~652Lc11EavQsW57pwk(wH#}CB`jxWfUOHTQ{U#EWS;Y9|@H`L~~pT(-aqG z{EQJCK(zz&o;;4DekVMIJuxR@gYK`F@l}W-fCtg|P0@MMnCG#Mp2U&&*&6#LqE4N@ zz+jp-Gzm9F#7?+{BU)%flEKGnE;BKw`+SVVKJ?5=_*-;3I`ozlEcC26E7AU(NTZQn z#C&K3(Y9#W7?|%KvUPkweqQ@U)1=Tsl`y|6k>8}uMYzJ4-EwH^g~B6 zhlMId(mBAKDKOjpc(K3yuAmKGVbm$b?H$#Fjmn5PvNom-gR5jn_Gq8WzaHWDbecAI zj-Yll@iiQMTOGB6Gv`eH!QiwigM@y2^hoQ87~X|&V}lkxPx`w?E&|VSrfB%B*ZrBk zqyt@_86p~UTGpi?8g5--Ee^Z+3c`}j9@++BHR;KU(|=qFe=b ze*d=F+U^VljpQgi*emXq?2}oD@xvP8838=<+jGTaI%T{0mC{BYQ?-~gmEwVg!V+7N z&hupKxB6fNn|=M`=!MW?ssk7C(LYqvW(aDxn-KLGa>`1nkwddK#FxD;pwy6ta`#|W z<%3kZ7^UPNb)eED1{g=NF*3^FqRnqsm+oJ+#pGn;A$m4M1XihpZL0(aUxl1C4up!DExLNymv#`ngmCY3TK~HwB<@=4~l_BEDp9A zZmMUyg8gPtpM|&?tjd0~3hQxwS-1oi!hG0({mz~KYR=YI+WK#>Ou`bY{Ap0#*5!r_ zOWg?tj;6<0hU8YEMzU$l{F4&ECo%tOUs*u8Fo~$dY>)-je7COSq5tpQAgTx>-m(PG z%n`H7{5k99>v7S4W|N=_1%qw?Zd+yP<=RIsREET|Eq&~BolVn<_(gvCLsvf1PckGs z&A!q{DMD+TOMpT^&l7W#&;Ee?Bx{++m}?1Y6}<6~feo#DDDv&&3%kgW5R%3oWdc0> zgA!Kv0$zjKs=3y`BywG}_D;ncp?lrI2x`qcTvD$y$kTr(um2vHy&`QOc7fE7vT|EL z&gaAVaCt7kuAJbK_7N9>U-0g{*!B)JtZj|nC1>VEFys>_>S7{nU)MYdAJ71KRA{s}m&&{LxJV^1qAJ1=B z26=3n&@g-9Xb)WNo;E#Yfks<||A-IzhhGDvNz^&^W4Jya?79!u5+IZj;z9quEg{+N zkw2_2U8%l+;u`qM4o6nDK~RvQG%tyU?d&bf72{RBX_> zE#j$LMS75@^@X>w#eb0Y-AV1sRR$@`zWb22RrfK9KVNu)1tl-dr_6n=*7EPH30(iG z`)Iw8UlQ2Iv7}83&EFgAu#fTNW8XHhd5BXyz<)QTA)g#*z~#5$z+S-OYbxtQo!hbW z_N9nYgF2e91|;y!p1Yvyp1uwkaX%hHkTJOal?M3Av!F0S#!o+?6dX+%@lVNnV!3ht z^RB+n9-tU2SzF`1bRcnOc#4If-i&LSP_@c=WuaU+S?!Bd8unHw@%CW=i6L+o=_|Mu zXO&2U70rko<-hBg89n+_TOf<#Y*v)5nf2pVGi>;&%Y&aT;pYE(9)S6~B749j)&V%mz{DKln z(y6fRI5?K;+ae(e{r!(?A`mPj;nEug-(K}*-yay+L;d@^Q9epziFc3;ivS`ndu>K~ zCP{8*2A31A1-(xe_~Yl~kGM%^{%n*i*k~?3xBSOW!i> z&ZCc%7-z3n{h)ss86Xcqp;Z-i*1rN~vupzOK=ibrGLG=NSDB58)_q$Ezi;1rR2wLRuj%dI z{(kxx6wpRQ+$LKA-yvKXYo;Bc*s1)&V1L}!awZX2Zi9e#Crh}z0S)qEWpkn>MhAW8 zzl~wX?d~Kk`b|Nl{$taRT1vRORd;?=21uI*O`4-_jiE%&w9DdkTDf>5l&cfdW(1k#he8-o3J|8Mc4qz~YR$=s=%cPhb z<(l5tcO8kP)+6aJqh-BnYaji&%`Eh&i&=$ek|js{V>afA)0-;g%{6`}Vq{o*CgS zb=?GsFHX*C#i85zn_?$*qz7bYx8CcWlkdk&3jDZ2a!sT+aU1+{72D_)h7{*r^_1Co zKv9&NP!Ta!%KkKzlCL-HqU@;b0Lh8A^^^YYwAMY)zm2|&f=|A0=V!qwPEL*44Ob6T z=jQu!7v?_UG_XTt1H(_c8&c{$eFY0=yM@y}Z6`%%d`h=90C}>~oa`@QM^?|iFlr7d z!l)^_RF~B~x%nbTM2j(6r;(_OG%kaf6dfRi&^`Uvy@SDb=urb3U-J?Du_%st8ZBzbH7A{qqy5C&KBMu*GbW|S+?4(c#|MI%;W@i$ zzjMkJ;T%Y#AqM*dc2F0>2{N@uPKzvgMH zT`h8G%F(&i(;1mWg!p@6b|CvXEl9bUZcqhMuH|OUe;Vs)b9H?AF;X%f1n73!R8F%2 z(Dmoq!R7b|pvrJnk*oY@Emp)%u=budu{+L)@Qd^-E9# zNVr~nGmP8tTxT}znPXsWl;Fe6|4tIxP{eAga-E4bsDq~PX-K-NErpW;ce7DLZyu8w zkFLceEuh}b*DjWQv)ap(R^t>6o`&zolR1V&t9==K^;c(k$h>yl%GZk%E`T4BBnkUY zJ63OqN6vj)UE;EgLwISz7a#d)q7-nXKNI8Q#{kPzWdM768XN&V!0RiZ?2)qZ(3ukp-`zDs+k>;lSPccDg37_HI%v=kuqJ;xBe| z0pdywWaaOyXW??dQ=c6KB}U|y7E}KL1!5IYN+}VZd=V?Q(|xp=DKp5H_P5F;w`rkg-IFF@}TXLC?8-PS_9q+kRCF4AuU1XtbliukL8Mdw<+=n zydl(PbTBAouQRgiRQ!lhas+tv=$8^CfSI~VG%!N1mJHU+KbgWI1=7^_>XN4{l4>o& ze@@@0_fT^&iFq+gXYG~4DZ~#}`R>#YrW(8WqG1l1t!f?6Y=K2cChkH!wi`x!|7m@x zlhbVWLw%8o(;xVCzy=~7Sm@_aU(Ww5T`etG1fZ(Ph&f2%d^Nu~@fp8Q0#SP^aaw#8 zA-_+9g+VeC0sPoLI&DuqSIdz6hIaLl?KBJIF|N?J$xL}KMNjylUV`WQ1rrlfQSzVi zjzkfye}^=2{^m=VN%8v!_KQk_Hz8($`UefL9W%X9<;LZ!5% zin`+X_>ckIAXnr;7ePZY2my*J6tRo0^&Pk_)GTy)MJQ-J)Sh4IJ;zuj zpUbrHPA;c2PPKls!te;c@4wht9T8=d_K^fZ|BIXT(w|S5ia@*QrSBS-i*{zx_D!zm z?}6RGO#ccPu&Ayqi_Cg5Ck(F^J>M89P*K78)t-61HpU-=JFAk!pwWocTe@4?hv5l8 zY0mHz!Dv7Zo9roonl69{Sgl719L$TY;V}t@?hk+=hPf^4JIaLT6NMf}8(rryY#k{d z6{zQNvE@FOfSEAK$?U))DH!~erMMxPpMJP6Ujj%$MMNLf>*Q5?)O-+vy!P5iPZ;K{ zvf)du-!AU2Ed?frG};-9&@R9+FM&!t5t2wpQ9sfOpmF3X{xxpT@HN zoBJfsYt1m?697N*Gft6;<*-Ndg^!{O#CCpuC%JDhabm&c&z4dq*ARL}buT!I*zj`q zv%LUKZ-&fath6Ptas}-QjfhjZ!{Xzc{WYIaW>xMmY}0Q|y512e01T)52vy6j#p)iU zL>Bn>JWKV~^z-u@etLiE9=}Suct@FU@>@W|iUQd}9Uyt-V>uM0cXe?U*3#Of6)w`d zFOim`++y0P6;Lzqmgv4f?m(HRF!-Bg&M*4a?}0G(U|?bq{m+N2B5l|aytgsu3KJFd z6>$Pi>i$U+kv~G9qrGY2_E(?rPKs!LjaC*~HPek=UpV$XplJzqi~M!;1OI1AmRjb!7f_9*6~uaB*(FElV`CC!l=zh#g-xmn*x1fswIXt#`Mp! z<{WQUmzwo(BH{h8<4HQK!yUdhuZscN(Mokl&4e?t1N{e#l1ZDxCpC0q?PAH)%Ho#N1BM8IPIMT4Kk1Atk@8j00{SP754P2LiBeH1YMdxmx<(I1kPe zV8DrtTYFzPv~U~Nx;4@|w$Zubcty#HE6&|_UwEtbX1$xkx23nnWcDEWp@`=R&f35t zwPQD!->Fj2PV72MS|j2g2HSwTBbA6GGU33oapY6ab=f7owhfv;3E!_(*Ol%h%+i)! zlc@~ppqDi+kzeZVZVEI=IV@l>FP%GbBmSwCLe56CAefG%yTj^h<7Y)lz9UbS&6R2t z-%<1+i)lY7LYfH$Yaax?F2g&tN8PN*{MSQ(5zWTVvh~E3+JPHDX`j}6$$>d^iTLS~ z1{nX=r|jG?TRf6d%Y)R+j5@eZIn<03CLxqi8YJGUl4NCtFDYKWpr_T>Jv)L9um~p- z{-G(bkg%gwtnhd#V@Z2vn|r0+`;_E=XF+l)TqLZ|b8Mq9g%shRED@;!qoHb19I1jj zZOniweT%d2>l}oZoMryZL(FwMHH?;Dw!-WDTgP0mt3*jRKPItXTlU5=hMNtO30!yh z>s_xO7=wpJS#qpp4kn`}WjkRNkrHfl!=hQyW(QJ7OEJBwX4KRKb`~_nxP~@NR^EO@ z&lu>R?GgBi$seyi$B~(X=wQGndG{0e41Pr z$@hR{jUOlK0rnkh)4^BWPo?Kh-PZuxyDNc6RJlY&$5K*S`)M}maPumPwFB9b(qY^o z2|?UDf!AkUAI}bqiy_9k(k4bvZdX1etG_p%LcN47pv^(3d0T3UYNY)MzWp4C@7 zGj-W+d+%2cevY2n8`vc|wJ)7)mSXNtyVmqrm!M(pcQyE4znX8nvC4nRy#=gBw%~?{ zc3#i*FTHPOVXwmY;D*=^?7uf2)Em3f4Q3lnic&e|8$jBxaOS3Tx9OD$*751V6M7u| zlqWNpGU$=I;f2Edk0Tp)9<$tB>bz+I7imBSDNRBu*iv&p-HLnIgg`dGpA}4lWnYiqcmc}!ZY|*;XwMd}A_ue0>&`S7>8F^bV z4}E+n?orvU#)=EoY>7O2<%WQ1L?xI20zs&m>qTZ+Y2uy1UyR>c zd=c43t%=HcsB%hM9C(_`89HVaY>M** z+Y9|gIZjxjewH5XO%n)k8VONOHta}`4OJGd`=5&OBZ9aBR9B)zzuUt(%SpUIU$n>%M6V=1;PVHVww!x`T3Gp z8=6UXUxmKzgj)!#7i+4NtlH1a#!J>67?myabE>&_hDz^+)IW=oZP=KR@GO5_NZgiM zZ{Mg7iN}6zCgHhT`8DIpgD$(FOb$x8TXpPANRsDb!M4R+-md+PLLBFiG+sh@kYYp4 za)jqDed?1B^02gbsh`r64xJHbijY(qeKUJ-^hmVf?8}IfOamOB=^hr&9XXsMsXLFyb*+LK z&hb$DT(m#Ph`0JkOO^8++~ix|WYR`&B*IxO_s1#H*DJG#rp6%}?F%x`fQ<2swG2lDO!AegY?fsIjd>w9= z4KgZ`Kd{t^Dss#6V+Y*Cy^&x-;?3RnR$;eVOvsf8LW=VPVu_I=Bx>WQ8S75sNJC zeb2Et#&^bbBShGf^utysAT}!uC9o~geU4mX|MP83Um_Pdbt|@92@0KD($PRcx5LHc ze9Eh*Jq<-V=ag}*)iT%SCz7S31h7|h{MMw2mk#XB5UIWw*;SvOQ zEBQa-#Pi~qD$GkPn|%aXL<%g5Vjwp4fgO4hB9UXL?6roLYFwah8~V<2yF!~e>fJ$c z!o&&T+G%DDx)CvwJtOvj4Vk%1N&%v%>L5{6;m7KjBc_+-L3mgE_R%aa_@drezZ25MUUgR0k0i zx&GR%O8Ve!^KPep&SIltPRMs?~~Cdm$0WPMj# zv&*g^z6v@0SpJ!}_H=quuI7XZjn6B!4{zq!lYd%{LC(;DF8rBwWzs(1YcuQMHi(Gi z8DxFa@8_$|s$|&_@w?BswDNENR@e`vud)a81WuDacn`oL=Cllt&Wm%;=cSj?#l+XJ zU3o0YO;(cj>^jAS`AK5vhhq=bqHJmD-JOCKLM~48B84q9(}BC-@;_xmJt>?-RSl_d zv;GJu;JRZ@sb&g&F^F03*?a{yVJt~I;kjbGXkdm^ryFHcIsqSY5W>B7#n_JKiLCM& ze`eFiW*UPp+~nCxMriLGOZ=j;SFmEXCCSI@MYoyVaOC*r4%?TW; z3)E;2b_?(JWd7y4Wg0qzMM-?@@_F>Ah*=gQC)=5re{VJ7r1pNke_f>+eAsPStkS(R zyXmMW^)%Y7qu!`wlist)Z$#O`x?q}qW6ksU_Zr0T_eD1x=zIGzMkYh@QYgZLY^JJx zBF~`i6UY0>`fKImXOcUBLj6_P%~|NcHe>|zmvKHS5b#(;oE}fwh>}Fh8F=ooYCNXw z0$ACIIdtluvBMRK4_!L6!mD`!N1&!RW0A}fa-rs*Xk(+XxF~*Ak)#pxE1xd!pf+*K zqQ3P0Xj>WW=%PLEIdu4vHu0Nn%)nIT9{=wHwjmrTFr+y=*E=LUHZ(0`bR^=UW(j8EE)=>cnU6Srg`SlAm|D1X<-2jw6Q%lJ~B z=c%nZ#~7cK#pH|j%GD+q7~CwU#LXstMZ^&zxV(F_(lT&QOcjb3`jUKg=_!6Yd+cV4zWI#L+|chnT)O#483T*c-jO)u zA-sTY(u7lfZHA2=S{f$pf^Iv5=FjYKKcn)v3XmlL!@8l5US|&^jR`q}v<5%v0bT7N znP0&$%4QpR0A7V15n5Wt4(Q09I%VGU%PaKpkuds!;h27Xx#P{oMStEz25}J~X;1U@ z4FHfzGRMenxnd?2?~LP1BY#w-ZqJf{DLd+wPi?RAyHr5GkZHy)tRQMrZ47$W+%yH{=C` ze%iby8y5q8sbCZ5yW4+q5~9B1YTI3EW_`mrvKs+h?DnBHYcaR2HZ2}nJCbN=`{}j* zMP4H#fANCI$T+&;H~cyBXNQ$Qf`Zh_lj5CmEI%}n0w>n{Sl~Mj#rKiG4ros-Nr@?I z6L$|~`NLZ!d+Yp!WDkaW>ac|k=e}8G>qs^#?DpHXzByi+vE1*z+ivEU^7+)^0B(=& ze&5TmgF4Cuk!DxZj;1rL^fi7=I3XR{B@KP zA1>Q4{M@$N*$6lI?fRZ4W~@yZu_kC&XJZUO{D&H;=O!MqA%qMX>ppc#o~Q z-B?R|U{_2WaD5Uod3TD9wJScBzcBK9Ry9akmWTG*jxnZd0hY7g<0%SbNi2VmK1^X} z?(;z3+FxqLL^X2!Nk;kVc-@5z;D#f>?opIrrH(PW_sOBAT?^&VQ_JP}zd9xRM?Q zn`*LYvt!5>m>)R45Vv-rAGyWGqt^=8&d-a!$K5bPU@Y!+VN~?;wq=F|A(^2spCsJ4 zNOvyt7mwFT4R%CJgDvosQdoHY`9{@Y~i;`6ZARu^I6+WGrMIGtA`j zpv_Cr7(pnYR>Fm(HH0qW=~%~?YXO&PKF4>+7$mXrF78YHMW&SMFrxH>2rg0d zKlBK{<1pblT9}J-XL|Vns!ol0JGoOy2V?Ufsl=ma1{CPPA%4d4&52$;3u6JgA6%+J z=6I^N`wZ~56uQcYX~@_Nd9FIQZ{DDu*ndHW+B-sxL}_`b^Tk(DNn z_PYy|nkY@))X_BvYRVIzIz3k#o~;aPNFLvvtF%BUD+OvO)7uBhs{nIT;=p*b+P$^k z7cNf1;eU2g!F=HIqPI_(fFz8HVw#=OAA`@y-8Um^2IY;!-xijI42@*#$-PnP_KDzo z5Eq7S)^#XnwpHmxU+OR_YEP%DUE4TY!as#)1x8dNmMb000`|r1^<_qfiixI|HUFn? zm(gh?Q>zNZ^JNcwwf^`bMCsy4*9P~~KNG**D+?FH7nAncl(_gDj562bKYOYDw zQ(hRU!uK_c6h|a%-NNnx+lQ&e0|+`AsBZ-$|I(cSL+zc9^RRD(cNb9MT}c48Oh8c3 zEoUjX+7FkfNPCS+OD7&VUuvu`tM1Ej$Z0YPZZn@Ra12=qmhfs{#D;62gv<&Q3;hL& z{)|46ufApSD8N%WkmKR! zfg+b5>{Ab%OnjYD^GP;iA6WatWb`E>E~^c;e=FO-KQ3l{DaB<1rA7P~0R?>6zklp7 z=90XDY&n#}2cHb!aXVWm1?;HGDC0pSWAh&)uNg`{;*f}=EiyCAeY7A!`w)`5sX)M{ zU2lSShIRA$&(-;lui1A&NXvY_e-$h2?C@Ii>$8U=1us%Md1DeB1u)JKJ39xvn%@fC ze?iVq6KTcVRlGz`WAtl1pWvU zgZm-mH;mORclPFxRJqGWSjkQ>?Cz^=@=}RAioCep)VFg&uHHLN9oL|dE@@+ z{`fxT&oVCoYD6=}DqCL{=gRqd%Bp|WzrG@W`l|o=`ji4Jt0rUYUQtwF@0ifg&=Bj4 zxoPu(B!ok3wvLQs1M*e7XR3&B;Y#PIqD=zhs`LrxaU{e{9tO7jdZR9ngDS@w-WB@u z4{3=^6|i?-By9iFz5M6P`z|TW#p~HRICG$^oD$ryXGfu-Gasotck!l|i>>@2F7HLt zK4X}fbV7n)T#)-!X?#8IINIzDrHznrsjnU-@Rm<{gm}pNV%2)kx?d_9dOY^vFTv(N z-svdmQ1)HWvM2-c0LI*-s1JPrBu-@(a;RofnN<2LJ#q=SYAV7YQr)V0@nLKmTsUev zvS_3C^PC+vO1B3cdC?xg;yZ#5B+H1GyN^t^@W^|e6}D5W(}-KDr+HWLYF)x$VJ4ez z%KT6N=XL$>%qQ^kLbMN$&%5jkl#u$}sm9Kfn|_PMk=C5PI4{ZD^jBqjZSv%mSTr}NK%J4zT=x>Ox_YE)y6saWHkepzjqQ!Y~9%##;}^}(leWTu0> z-kw|??f_N*Kl)>kh|pJpSk?AMAf2uZYrf-OP@(voL>_4#y5gP7u=<7dL-fVR3XiMF z{%)}U;p$6Iu>MP$F$kW{U|t&()GgY;M{4%+iZ#mYDW%qGJ<< zRSGXoMD=u6BGM-u@Xj@V5l7;Q)W{A6H|*@p`Q{#1gZ|_Hy3P#FH>5Qs(K=30-Fz_3 zR%S0^=x*F#s-TkTisxC5(=y->#H}p{#e&-x?cxt$_}esJ39PBZupe|4||hWV^9Qi&|n z={rpL@qAIJeRbTg9!>GB_NB$leD6yV7`<5C^6is22t$e$do!f?t_K^#fO+tk>Rn(< zzMMm8NKWod)n8vvKrh&C-GqWNQ3LF6^iLOwOFd=IW8R}Lz<`#~6mWr7f~;^s-2iq- za-7S5seJ#nzyG@@0B55!iS@MPZcm)EuoLE}hovp&hilBBB?V}Yw>~nU8+cv5v!4F` z90^q;vm%zd68ciX6eEz$AL1eg@WvQmzt)vv2j^&PZ2b9H0C3hPSU~ggo|IXk{Vf@L zwoa+p8#T-=BxeLEyC{U8Szb6#DJy3bYBM5Zyq!j@cFB=;5t--yOBYhBODW;AolCeH zDl$P0UwxmJTRx|juzXh5#PE}^UnX*T7RJ6?*JqgNQ@dHzEmLv52~5>F*Z&U}M4QAX-tvWW z*nGs%^xFqCQ$kp1KpzqZ>gMD+FnD?ufcUFyG)*ATb7nOV$4p#P$lYdd8+Rv!9>M|jUKM^_0XVt*I4Du`*l$gezQZ1YR}V> zjc1j!?lrU$&}YY&Me42atB6!FQdTk7O?w+eS&>|&dC&V6oYOIb0&AU7TL4SOfLXha zpVJ-mCne7~VumBn&|Y&eY)^*4xYl!XHdf33fWH4>V?ekP^YU~X2FOA z+OP<3xlORji~!b*#_{$fyOE`L=~)+FxDAn709xhp*MyiksDC!{gmT81Op)%XK-@e% zSQRP7zwSC7T$Y99kemXYg24Ft)NO$GdDSX~3ft@uAYfqysE zAYAROtBOh67^jMpZ!eO9s+%`*nr`I!OtdlZT11c5x*t4x`7%?w-Rs2z2O4StGr@Pa zxvem*#o3QfeWYr`0cC6u=hQ6~a3yO*8+QP1Mu{YBE$tD}E&YTflzS zP0*na>^ztlm+k1$z)^0d%xSz<^Z#6RN`n}gWKBBQZwCM`IQ?|Qm0dOFZSFfM(3;lf zR0!IX6>*`#cSxG{s6@24#>Q&hOLe9IAB?nXFB2HHy#XOCrSvUMHVkrJ!(X4T7^&!7 z94DSs?Evz_RPNOgV57Xh|IWOtt`U)pRtfqrk)Eb)c9z>pvcrq@^-bsSN6Vd0p9H#- zSKDL>5Es#Yp^yog*F~SZIW3}X<5Xfb0>f&XlQ-n?n#^^zbL^!YqP%WNFu2<`CY2cH zV_fy>*-+d4!AE5THXM$WeX3juT^mUG-+%n$E2xelD|rYFIYZUXH21owW1D+T{Astr zd;LqUN)rgBNT7V488l5K{|m&)K6G=d8pqm!lD}N_+FsJ2zlfA}l zMmgosMps~yQb7EyO7k8Cu&!qf9>Rv?$6FlN4O(K4dh44DH;7$>=pL6-+3_Yv%j0WW zp7$x5AH1)x26%@r9r854eXv7{J|GZ42zT!wr5j8+Q0~wp$RrPeK zd1RZyOD+MER)W)`QNtc59GpWO(;a5suYI*Ig4=-2)BEWfR~q2h(mU301|F4^RW5FB zmCjfW6A{)i3C}$dYaKg`i^|{&t*X(>0Fp5iGNd>}%JbyfKYY*wXDhSFznmjfwdRZS zEU9QrqlWYt@lR?zf*VS_7)*7w#B2#y#gPBwqrS(Hu2iJrpY_fbn;d17JqLF1_9UR&HK-Bve;J1eDCN7=XsXV%y|A z*KKdXkA~R{t!TjL7}lLyh5diNakem;QfsA%NvS`6Gaf~w>XmwVF1MFmoo#=GFZa2r zPFc@Pz?HtcBkr-H?{Fgwt1Iz|NpnKEa+$*azvfh8m;qH^8{ruYgKEDJQn8k^&sV5o=v$Qe`HVDz~CTVD0#3xqEq{Pq&6v(px*!ly5;d67$Shv8fUJ zKi{%H`yj~`SNN;7qB#7aowK!S*@I4am47dHT4?r>Bk~x5Ym(5 zoFZIM!IvtsMdV+;-ghWil_ftaT8rsNxWlBw+q1rZt@P z#G`l-F$2@OYe^UZa{IBHTn5V77zc-8=$1;Axf=}i-32UGJn z#lvcaE&M-V*YCdke;j<$7i2z(c6OiEc}fm+P(-d|*n=sIlKcr^A!q(=1j53izbFH| zLN30^>ahmfmJojlYXC%pWx(L&HyCb%z*xkdldCH+Lej>Hyy^~rjqtQy-E&P8aV6_OQ>cx3UwWJ+u>=dq;1ZS< zX};=75jKjMEe2v{YDw!v(p4}4VKQk8i+)SSmbe&6C&pG-clK2!Lv&dDN*|jBtL`|N z8`#xJo-UEq_svi73$Hd+?b>B$`S$N3@UHvrgjpLkV-U9L2IzA^}n0G z-JGt`0`x(Zl;Z|uiH(Mio?|ia;kWPlvM)A7a6?ivi*9@gK+`w4nUgsvi{yOBFdTL* zzKo^|n5&g~O;tJU?{gI@0lj(mw`>Y)gYq{yLwSaVGZ}&pXJ~~US&rntK87v4W$%`_ zlAa4{xQ?KYv=}JKsgCS{gdij`+!cVWlN}M4!G{3PO7qAIuxb0r1ObvwM@NRY@Z>J- z;<2q;RsPUb3#+js98=W7hnQGiVoMPh2D{wv;zK60wJZ<)mtN|Bbx#e%X#GEbj;m-$ z9k23&hy0<^OMuM&EO#_#PL=wKb_Gz(9K%`o%8bj#>1w(F^&-hPBO~-Z`hkr=%52b1 zv|6$uZ2^H-9W*42N>V?4fWS#*G)clL{(kWEJq1jwu|Z^rRxA7{UOW%2H2l(%@gS3k z#IrzC@OHO2FeM6Kem5Fs_0crkLN3JyCP8!k8p)_5u1E~ap?rb!_J_e*Ca?Mu}~ ze1cy5SG~;F4$gwY7p1Mk-Fe9pS53rHiEXRHL|EwfBtMG7Ca*7Nuaj&Hy#R|*E02mm z!Lt%tIW#15<2*eC)u$vGW{aNmtHz0r$vlFVjg0SL*ZlLHkgp17M)0guOW*n{oYb zguMKut$9w1CWPlw2x-aDq@60#u`b&83iL$nz*vwmhD}{KSni@AOXt_{SIxM+(Jg}@ zLORZ@^+j9AN0@mTY?FWOGX;0JV9NCu_6#T6yIm%N=NE)FO&V_BvATH^zX=e_K1zrO ziK36VdfXti6f0}H78~|;UGcm^0@OvDVF}nog88B^b`0_9cE_^g?;!wbCSaXgRLO`r zg%=pmqxGR;QQ*yjl4l5V=cUgYWd+^!8ap7X6H_dbw|-ywe{_8XRF&PU;0^-S59+I2bxqc+Ru;UVE*% z=9=?c&Rz5WM;Mqp_HWpS9YtwP4=z=)Oof$t+HtM5Q_E3ND7NY2{B?i+Kb1YT!CzRI zJ;xv)?9BtI1!uxvc-8ZedTKJ#vwdS16lQfnakqhVN#mhp9ctOVnNl&x)wN4GcEaU@ zVKEpGs4G*lcF~{b0)mJMhpng%s^v#Y99J&0w}7(D3$&(;yYBSo6s>J={e$mw+#C#E zHw|w)-J^UsJ)>Nshv>QFt*Vufx}aa~(epYhqy#%Z?H5sH<(e1@BvbRW1>^0hQ}G3r z6CHfPJ7BtK?Cf}XbNh`4&Ov9*I|ci5!SFS^fI%Elj#rH)&m;jT|L47`@fM5M^u%@ROqIAEfj@FW?0UxGxm% znBSVich{JxHj&W?gIPoAmcis!pe*OYV>{Qdo8NDQR*eSpXtp~@M_TQDyzwTpS-Jem zEblW#s$nA*kd_m+@@!HZ@2<12*zYQpMpFBq(j}{g~ z8Ko3F=kQT{alMJRpIfY$_FkjPy95i7v}DD1PnZ*?ltKMkqjrM0r0cxS8yN0o)tW8< zPVe-x&AipL>A2PvZMZ;(p^5eopY9fXfnK#d(N+km*^a;Z*Dv+|t+oUQkCggYaQz%! zl{_Zn)Ap_gv$nm`m%&}*c{dvj9UXa`boeAD`jj4yY#AamQ&gANkFaIQ_J|8J#Sb^PM1P%xm|cxOzc* zMmV3BHKbr6?w4M8gM#FVwh>(UYuCtP9w^=RQ{{`?-R?qM1+**IZlL*86lL1rWB zukk9kt_=pEL($Vh&qY3vNy*hZbC?^jFYa|tYMvS<}q{P)iAS4+k`hporq)mZyv(RDxP2etv8<*DC<~we*=7 z^`l!L$Mjms`r{#^nDMc~Xj3QcszTy3mAr{uJj>}+>)wVi?2(omw7oU{GLvYvp9rWS zUgBUg%5EXw88u)CpwO?fsOzH$c_xKV|2I$)C=RIaSb;GJJv0@D*MOuf(;`GLUsQeO z2V)SlIFrji!#!Gl#HDu|I(NJvk8Uqn483l1LuMvCO-jM9UrIDyip&z>1A3Vw9RXW^f1jT{0qF`a1 ztR@#6pRI4xlH7PoOMk8$7>+Apk`75HCQtiE3*goE(c?~@1M>fjhkqd%QgqS!>bgeI zF*4{XdKF6CZ~4%-LSFxq=J#{_LxNzOm#`Ypc>LLw`4lw1zvCa;dj)zmW$O1j{{JJ2 zs0B6L4&{4c8#*p3XWTn(-ij3Ui$XOdKDNbhs1iWI8p)HKRRg}EHWlv&WE3xtj#PbC z?+j>sqq)>$`7a;1{{=+&-kR+R!eucQ2}67Wkj0UZkh6Y?QvGWE^jP0e>VAks1hx9e z8|uY(ODBXqj>ta=8sJy>^Z+Ew`zS`;{ zT=;aiQQ4cTUSO?1fol>?G!Um<_G|3h*o%tZp^rgl8Ps-mdvm7|D(7!-NQE!g9|^pg zt}rLymMS>FFc*s<>~(rz9|L{kuDkL;2PfL(9dp8e!S;*x{MzCSuX-1~~sUB|W4#g7>9{lu*QZQtVzgHj7JcJP$Ux37Ui`KdXA9 zC~x898(#CeK<@e6S~=OU)&~~MCg1ihr2kIS{^$DQ;`jp0R@%bCA`@k@{bheiAij$_M|Vh*$8apX;>cr`zpyv7F1-#gPWLBt!M1(50W1KEk_W%iq1I#BY8^ z1#DfKY#e5BdfZqgaM^}=?Tlw7T8(%Tv@(f75Lo+Qr6m6)ioZ%#L>RVgE5{BwwA9bW zNfD^o__9s>g@06q(zkq2P?%u$uBe*gT4rMf$iKAC|@HhHxF zVcU@R0p^v}T!Wj#d0K$xV@Qgy*Ge8!Q3Md>R^y$0H~Q7OW@lUridAk`lA;lFz7nml zl1*kCqhSi~mv)R^VLmHydAw)VD;Ut-=UKvlevMvzA}rsxCPw{{<)a5K4p3Hh}o=o?T6Q4&!mf*7g4V6S^tbE z#+t2jiWbpvNt&Gj_KXJ z-sO$Ek|gO6BBC36Jh)ZdNM{tH;!X2>+Q&Z>yVDwH>bVJohE|3?lyT#PkjIa(neY6` zb(aEZiUyphCtuhV<%G>m$?4f#`w_2}^;5VhvUx9Cf}{WTd3=K&rK0>~?<{U5(;nt* zUb^HTK`GR(9a#+!Pf-CSziQ`B7#XNo*P5{2&A!VBMd-l9M-RSd*9FGqNb@uG%(P*i zKsI@#PK7BEw%~^oH|e<6JE|bUqNWR}g-vMByH8u6PMUUJ4@pva-Ov1tV$T~?f`!Go z78*sD+WXE6OG`uL_z^|HHQtPeOgu}a@bwPj5Lf7)oZlJb3&uCL0~gL{A+KUt~fQ8_jS*wM8<2N<|Z+> zR6vVQ-yXqCiakP98%Z0v&O^t_ZTG#;YOKsf_*3<#c7An)BOd-Mffcn=P*#+;{Fh&t zbgInekkJNY^vbi^9jjSum&|oN|I777i!SP|r>v}OmVo0gjEKJS5DwYZWtoYpnD4Wn zuHT1ikCU?T!;(#41(b{_18crHDJD^ST6_3(>C+?Qk?(`SEUI*~RWf^$#Is?BhH zdBPOASxTBEx{wvG8>N}?OJZFgJIib7TaIxsOH~AD&-G*{+Gt{^92X*@@OiFPg9zbk zB1A;k#2y4Ite%_VW6?fl)@#xRjiY+^fU8RhY&*w6GTTqdJh;^EsSI67?q6p-`Wk(g zIPrn7!ZRcX7INc!r3hqXIjvVWlSI$Chdhh@V-naD&fI1$0v5N%^Y0fgo5rdpUK4#? zDw6puUcs=5??i?b*Yt2~wEMJC{BYG{O#PQx z3aOR$#mbxT({{7DtN030YR~|WGg7zXSooC!UCn9{m z8f&Zcczr2QYYr(v5y!FWvd)~_x1J&O?3iczp+profo zt+t)1DRnZ_^J7BQ5P;^f?nhn&CXD9q*dPnd?B=b3AKwQEK)22s?siIV~{#8}E-ngl9NR zYpxDhSVG`?@PBKC-gqCiZ#z}A6I^_{uP1b&P5I}N6b(bO9msp>@A+)18njXJ_FeWi zDp`rvJ;u7&gm=0AjXa6IMoMijZWS5cRQ{H7|Mc()DrybqzyJC#3Ok0REzW;<+7wa> zH8tm)0P9(B*%R=hFaKeG{`=>M_TbwQZtgmb5mJhnH|ReXB}Od4^|6fEU_3pTQ@K29 z@XhsjfALqfL(~Mj;fFrIPbocuYuTEAmqCC;h9zwK3EH$FVf8{SKyajv1*zw{&UX0;mQGROGhW@XAtG(X$K9oL~ z6p&E#amHq5i$UoqQ9}HdQw_~|HG{75p8vT_Dsny!qAD&TWdAUau+UoS<&9wN|60NS z_8Ucaqy3p82UK974MdzjR;){F z#g86C=i3oXER$GeEmQ8GVd1oX*334)GpkiKV4SeD0%e)=RFYcA&n`4Nkmy7zD zuPVj2M!lrMq}Iz)J@e4Jc^TX7`eY(&i?zPnG|@UvifmA8}iMqUyHbi@E}w1#nj;0e|UZ;Nc7~NtQGLESQm6 zN3*_t-uAN|`ji;Jchj4yw$&@PJu;4U+g|0r3guh5z1h39O7151E|k5k#JZStI%)uV z#l}nD136s=$uG8#hODQ#`~zz2jv8#+kopcM&5|RFn=d4)W}T*=c@ytDTrF`c&zay(?2YxEKqUXeG>|CSu!Qv;VirA^-qWr1n zwrZ;p-0Go)w|ML|nh_RX9M&D}dB>}TzHY#US~WGy zt|^c}mr6$E9c`hFA4eR|OAN{s!YS`;lK5f6V&bz=vj#E8&F39#1rAOw8MLsjBp1^` zN{uS31ajjTzrBW|FwY@oi#q`$gbwr1>@K_77wuW`n&M0&17G=-<)1AgZTu+D=?7BD zm$&C#24&^;jczwrivcEMd){mWRGx}ph_h(;g2tC`XkO1X?V4qBltHF+f4@BChWlO9 zoS+^%#;?2Vf%V+SgT@U4(pXNX9bt4*OffGu~?L8GW z+hlbbTMHuA>;&Xg$$MJbTX}B7A>hZ3hUBXayuk0ApEOH)+?8;|wVy~q=X%rt@=NQ) zQdj3tjaSodT@=U&I-eH?2zsG1Y)!waNOn5)euLulCC33X-Mqq5e2Z=Dlfb;M!{79@0u}?*sAs1T%t> zIKm=1x0`Asm6mFgmV1pR4I6y_SWq*df-uJLdqHtm>w_FxP~4Db7*Icu2>m2fYups2 zSIm(1WIWEd#4V|WN-hAJQzT%r43Dji=>@ zjO5ps3W}83>ap}TwLK(O&5#MaP#GNU=*YOzO zaR?}laT2TIr5=pwi8cX_Uau_+Rw8pgu7!-)<`-5)B{@&^%(;WQw_u6vY?oX5xq1jN zA?i9M#S7!m9(zqmRH|rY^gONE++)j@u&LzN_0ArI-VYV0%Devatjs6?Db1iVA8*-Zbo-sKw}qOzv}~AtrSU_- zidqg6kbm|W@;-`K`Y5$Ek-ih1b_Hzxro*aP{?ay`=D?1hl0b* zyrECm$gir4HK<8#o2cXk4_*pl+=p!jcKE)SDnP!32Acgk85#22yT|FpHO5@zkFKuh zF|z%(7CPiTNZW3FZ_iF;@kxf6s{LO0+|7I<#xM|Tr!bfR8y8IdS^HAZnq!FOvaI8x5*HE^?B^Y}~OF-pm?W*L1V$#8< z*Y7Mju#tJS--x<}+r=qqwtexqIE1Qlz_u3Kc>=Xh14O!uL!$0sbk~PEarO?|RVt}J z8X(fLqM3jlgdbXFycuI3QvYfn`~Fk@LWtp2GP6j7%;(isoh+YnS? z_Msw`HV_b{R{~5A5QFjT^c+i+zUG|>5y35li6O@L+;W`ZYF1mAuuPR&PmqCL;kf>f zb~5^jD;(i-QA*xm%%U>9AP;ruqlG>XTV>+yw`ioW!+T$NwFH|5XXhh_LbV=*JRz(o zgD%gV^$`q3;%KMCPT0I7fwKOjKN+7+k!7YooFBSnRY|FRc-f$*?)#6_ZYTpX%g{ao zOe^&k_x_OLR2seH?zOGrrYPHfGsd^wvGl@5mpLh=)EB0Gj-jQwBA1dPWtcJk%=h(6 z_hXb52q#!@v@zPQ@w9jEy`W8ii}g&k{DxMs6RGK}aj!>xc@SZI-YCR@Y#eG!y5*+6XYtE%NT^V}wrOHw zG$av^p$cMs|7JD>`5@Gk28XFRp3HlQ?STxj`hJSj=_CT?yP8=agl!_D)*!eJZ7L&w z$S^!=_erNM55kP{F;}#OS2%1IPG%}PG%>cRGw*aw0pUl1MO_BZ&Ka z;v=U!X8Ef$0~trGU^}E^cp{=Iz919zO+C9-}qy@lzNtvHIaBQx@cDB zlZdx%_iOri$IX-mvFW%J4v+gBO2Tbj<$7`W9Rd6Px;h#LX&Kb*L`7}#pem3gnkzJ` zl8JexkF1`5L3tOB%Xt8{hXIt=yKF(U?bQ z*tQ>D*rQd;O&)&0Z}vNAE?#soeZHgCd{fypaPfsj!IST~>qX#XBtGxLkuP%lM85{g z+&)0r@X&6Wd7SPMI6z70sQ8NXl5rz~FR<;8*8XT)h(d95P5I%PH=3_^Y)yVuqe%Vw zhR*ll=@q>i$StZZ&&?<&9}<^S50wo}@~}YR9^-BEOl$v=ljCp6ioBY#g%mzvKxII0 ziemx=p230!V!meqQl3AgmiN;MoiFGen9w0$Ykuh9(p`+6#R?!hqk|Zv*y3b-_svt3 zZJ+WOn5yJuWIj0%@`1Dbl@mHmQL}f4=dAEL=B{yOoVOEqY7*Qs8TDWylQB=0rbe(F z!^WdLuPB^LFqHY`GCJ!XU`+0jr5r+9r#~XE`lyoY=f~yMZ`b9k6o@^)qtRZOM4nML zA+0@#7qUZscUCRDZ2y2!dt)2)(0PThwSVr{*4iAg+=2*vZ9sriPjGV)?%nNz^X%0e zdqB)mi9Exi2LcAvU=!O$M(xCC25lRe@kMKGpf1f**W>jvhh+3GL*V;y@cFDkkUMmBu?4vIpZ;T_}vhaFDRyvW;{#mg+(YxoCxPp13Y|8H}zq(e=q&|GP{8I49r#T zcZWf-7Aa2!TNHFn)-xPNdg#rb5eukNbqIC+G+8z9!R&>Hr#19ZHO zTlW|IJ`O$tl&(5SpiUfhys)6|7yFjYmq&oEC5HfgF3J*hkmo)3rJcA&uLTIr>cKrN zz`LO%BQsIuGMc@`$>=ZQ=yk`cLEZAKIdB^T?hI|7MM|oBMxM^g0*D6X1ANst!#uzt zW25?JuJP=EGH(2lr}o}kZCkR`i1th}Of1Evbej^SkM(3o4EQ=7R}Mx_No)Y>tH)vG zvH4i%y%kSt&O7C5lt=~JF@lFjReZ?II|0;uiW|qh_wQkn+VvndUzO~4VF&Pkj6L&& zmy6yy0bo@W6_rOp%V8UqVojSS@enQ%>6SNxk>3X^Hx#Kv7WpEqE7KIDi3~C(5 z68j=SC7iER`_U<|m#iF3-%EdepuoF87<}(Da)Y*PIZwu(eWH!HSFXM12u)zpDKdM0 z^u6cgf!^~MMS>h*20S9h-MCw2=*)|fB6{7iW=$27$!opk0rsu_BA!JHH(Pj*Do_*h z^)YXDysz^G6FXwDi5`_YuG_BSuH0P&&yVbvP5KDDU`3N02x{i)B_{uL*Kl)H`Qfa) z!YT;|9}3Qx+Fb8=COE*MT?BX@S>C>>-COOAb`Z63hA21=_|Kl-WtcU~z7s#C;Ev$d zkD63A`3BEF70HuZ@u0s!_3i1LMH4=@dsH6Fzd}gVQz9`p!?0@)$B5n8F)!`7KzsjG z@ur=WBFKqh#GrK-g}lNe++G<$O$beq_mOTBdtc45FDEG~8#0#Sf*3r14|!Fv&qU)u z+k4IXd)Cer4f};xcnwR#DtrqI%IZJrF}wvu`YYg3cWo@3`BQh$@&{g^z7SbX@oaGW z=p#k6S%~3oxEsM2KVblKA8%|>d|h3dd`wY^4)ZKp`8lR6Rg(?>gz@m zZMiurrg7KR-c~py+w;M8*bp%tr9%Y zsrH-nhto?TwcF=M{ELv?mrnf(g{vTOJe4A#*VI5f+5|E>y)Mm8T+;k4-fvdE=w0P> zd-JJ*tN8HfC^*uVRTi8>khwK0$UDvX3FAn78=fF}RNd*zJex;A2##W`nEt_1$H*+h z-W3TG^e8kK{X=Ld;pKd2;^*K{C;sHcmsc4f;U2Q2`WIderMk0y=VNp2=ij>GKavgd zN{{S3dl$hfuw$%~(n2VKYa(2Q*T&b(C&FjjyyX}&V*f!Zpzf===X}ap;=FNsD+^A+ z;tv$^^{qzpiAEX}^wEn<++Y(jd zk|Nw><0sFz(_w>n+<_Cq?wG`1P5p`OQroS`n22680r}qfO@Jkh_HxORCyiQi{Fid1 zl3yiWYlFNZEBs(C#8X3)Y17L3_S@v^*~yY!i-OpIL`M?VdYFTUsc4hzA_|6);X;e= zMLG&3nQ7?YY|e7pb1UW6ir8`3d8Uup0UB6Ae6#CPR<5)?XGr8umxx&CR}$*!>gqpZihwOXx)M|pQ|`4s7dMT?LtYrahOs!;gT>yFmj1&*9*USLpW?6e~+&v@Z%>b4_ufb7i|w8 zpynDbwqrfbhpQ)k_8A$g*{5!)o>1`Gwcgwg8~#JIM`#v1HthhB-DjLlgm|ut7sG-_ z&}Cr-CWY-ApT5Y{B7a3QW`u>WpHc%#>UR2_2LxqO*7R-b+j7|bH8$Fd^2;T`^98?=$p;FWCc@b9ut@&5K$+!L>M`0&{y-eV zpJep#3|R}wbe~k*zMe2tHl&>gc%-P#C&S~4U+3{F?6{zdD|dq?C94kj&yW8#=vU z%Ld21OsO#J%B~!-+U=K#E+Ur~gMi&A&tx(7eXGZJvAGm9P76a*U=@f32biVZ3iWgL z@!@+541^cCwmfTrr^~}Dxrj$0ac{{5?VBXV(zFjl>Sp`H|G>4JXaIrTffTIrdyrd8 z+IDet87KdMge@&-usWVT-6XNzW3PVw7%r)?uDAG4clh-V>v;YY`Y$n3NAj_!z2}jG z#PYnXl5HP2I`uq@a5MY0H?EVjl;}1iC2D`ZHsv`e%&yAjfaC|-MpTUqU^8&sq2!`a z0+3QpblG=PcJqhw%T>l`$a&^}D>M(8w^$;}1Y1}1+cQM$rk^7D{Q)mh;x2yjKlrtI zNNqQ15`Uq33?Q_#MyiehizfsHd0B<#73E*fRh6~)<6bVoZsaVP)3pB_72lDE5uLU+ zl`(gnV@2ZIrU@Io2*o##BRewdo8^z(Skkx1|H6|;W4|B0Dt z6dZOE)oQCrPBXwcuW=E!J?s$RJ`j|j#PD+Zi}C;dV8zp_+J9RRZ`k0k2eQaA=?Xo5Y}6<%4HstD3P$<$lY5Q#|d72!#q-dqc-~8p5D^)I{A6h$=+Og zQmbsvL}-PB{^p_Y6l*1?+1yuYE6qmUO6ytGIA*=qd%82WoeHFEh?iKO>L0BB18e^| zR%rmbM+;@PL+FL~RpR*LCms78BAMKQ^PYPnh6R#(%*iY zo<;NYj;#Rfy-DQ|5mpegXY1Y(#Pt1dWIRg(99co37yQ(eMg6hAH!8Izg4`GGQj$BU zZ{(L1OG&`Igcgz`P{sHZ3`cj!HwQ7&p5*K1!)k41Jc@su(chI0Uts?+NwDDgG}8eJ z5|F{Wd}BCDTJi16)Kuz0!#`#kw2Jmf45%-svtkg_p&HV)O4tlpbNYAmTcu8@K-xu54MAJmt4fB$AB8dl z52Y%%MSWx#LmvMVvO!jsiv)1I^d0eD_#cPNSpdO{d2L&A$1+#CYyTuWE!gCskSx(^ zGn|lsA1|E(cfV9o7>9~qyHn>GYb3_mTyu55r)jp6c=StOsf&Ip$CC#e^TpnVJFlI0 zH;x4=W}eTSed5v=^=!II@xE~5*WW|5JFivES(pG^OBui=bK%;=Oz5E&E>hj?zqW#S}eK$3yPk zf_V!m+3e%EzB5|IP^9C2Ert`z)b5-im<2hCm18Zz10lnAh8SKoCcaExq%lgqw?6dO zeEN8{>US&W6+?5qOZfN~&#_#NgffeM(%mBk5w>#$u{4ZbFusL(bLdYyjiCdHg4S9q zWNdTyzUv|&V$pt%Ac#St@VqlhOP+@%uEwZfc#9grCC!bC;tv3R2-fj_*H>`U`7B8& zwvUCcS?W^sWP{pAfzBl2Upv z(-SA%+Md!U*qJ(WLtw5_y*f7VB88i9Myi@;<8Y=(=I&u+jRdo3N-~wQ1(JgrZ-B&| z$;ODudnv&`=^H4aL0ogydyW081>3PZH}n(lN*G1vh(2oBm0;Kw_iD#f77phHYlRJ2ucFdG53f^OJ zN8JRheiES%Vlk}?hDw4`j>El434}yt>k6gf*RW%LfA2RrP6+eB z|H>S&i_#|9|Guq2&@RAjG6FENnW}71=Gr&#pUb4~KH$s5(Jy2b<26fmc8)) zQA|7UhNs)bg<~$q2f{4QWa_61z_;nr=B3-DVl=a!L_|&pqf_PM4e&t-ZF{G zX>JZSRb|5@6UTNi$GR@$`X*ic22f;-VFqk0pQLIZOaESqQ%O>j9Hs1OQ(Z{~pCYGp zvtIJwJQw{=KG9Y&p`aSFE2`e2-r5E1Mev@QKZz@GFTMX+oQ5S`=ATXoPcS{=FgXF(g1az z=mq_eFHF*zLnB|;AJ2|w_BjeLhL(*oaQ)sI9WfUHBHF9iI3A5aiY(!&eU!HIB=m5` zafrutknB&0|0xN=aJz60`EZ0(noCeZ-+j&OY#I0DFYnx@5J6i$v6_2rdTG|f=z>Jl(odLm8`P(&3I(R&^uDt-9+%BHP0Svq+`=TQ z=IRRXdMwJbImzzQyR~b&8YK%}lTH&lJ3UY;Z&<0|ehVU}QKzg%siD8;&4t+^Aa9vo zQ|gr&n`bx3b|PzsP(6&lS+Mo*20N;vx8^HZZifsK2e-9EL=TWWbL#S zcE)LW*gSUsVvcv-Z7s=RHNgy{s;v+Vgj2d1Ytmt;E(Ks+#a@TqtcmQ!>D?T@x!TSz zZsL@N#e#-rN>80r%H$T9GB+84*0sE}9WRJq+04Iv8~Xv1h&h*-A{bRo*DY2iQxMk7qwB$W0E8i%tLdyd*$l-``DUau-2~NPZ|cZ z827Pgb!SRNbBb-_c-OoEPT{m3+oc|>lx$#E-2H+BG5gc3xGaC_;u_6HZHN{ak_3?` z-c$>0t{u7Gkz&N;HMX6IQo|{}k4qBi?{2zc0#fSmUb#2bmSl64jI~6oo|PpH?J>9@ zPsHY}UmnNZYBs;PUR|cvpS37(i{0F6$GO!C6S;BahsNlQ=Rcfz+v49@puyG+HU~41 zj*WY_Q5LK;R~!vIPgLbat|#sqo#l-6{*6WPPrJxuk8MCl#Q{vEY34(iDPc>*BGpey zTzx1I#6I35Vx|;4?9MoxXr42Ty{vQf`Khl>d^jEXP79ptj%0~R^O%HjSLuwI%s!yn zdbQF?W?O-2dEW33DMj=FxL~C%&5HEq9}Bt2xU()-@o!Yo>F9p37N0b);QM`ArQ)Ud zTdrRoOg;AT&jOu!87!Mk^BUJXtS6TlA#W`Cp#Pp_RyxsDM-~3~1wp@e$)K9dwc*im zw&W~cNx5ioqkY3(T6ciBcZ#l8L%8&z(EW{)nmc7{Xl_PWl(K?QhvvpmlEB2c%lW>Z znX^KnM!7_?`=&Yn@vs<%jSRS83av)0<>~~j>2*gL1VTASNfkhf`7S}^@<(|8!@0?_ zgU-ZmFrk_S*p$1t%m0WauiVio*wWuTqW$yP+T`wH zxLne{2W*kl%#wg?LLchNif?DYb-DS2b9cnPHl6T9w(!W0;iyYnsB`xL1F^Vp3%Tch zE2qn@l^bX~Ll66kxhHO80ii^NcfO(4s!D^)DE10xi%E-SNtc5rG%$Ib@9sS(R}mEd zuMIqZ1F)=lVgYi46)x(Po#D!sBhMw)ADal)GY(zqVCIY;;-Cy))V{vfM*}oTG0OobUPx`_`LL6|U9nfDk&Jwl37aZER zywS#ZSA+U~s84(Rwqzzi9C8b}cI;&8NcJDP1j1L+fD4R-x;*DHe_i#Zva+7k#dpXy^_ZrB~YGgbi(<6P;H#NuRA)mpwlyX!6G&4c&%XUxH)74444((DY!|xT&dKV6W0N zQoW~i2wWGKNcjON<6+ZDyw^O#gPlsC^Yd zNczx5=&_QyW+OFZqV4lL$cmj4@Z;%7o+b(_31gkHQ<5N$$2-S7p$U32doTn+_ zpE#u)eE~QD23oGTM`^%v8z#?Hqwxl5t%X_$n1RrZzmHK;u(QF6)or*MGFEEG;gaf49wie<-yQQjl z0uQy}q^b!`a&%0imV2x%UxVe$4*OO@{4)>r>0&mA}XaB)$NADL{F|# z#?IMCA-HeZ64`QB)f&fJ<|!F-SKH;`rX6a3PsCeMa3TMTWieJ%oc&!!_1VWVN%_;(m#4EkEzhWWJsODW78DtETy8Gb)(U`KjguMaO33}C zw23_Gmm}#JVWd2CW|T_5%b3oren51`ZHXh)b+4y_SNF?mKG9J5Ugnp3*x!209%X}7i%1?iuQsoXT zaG*$DnPJ(fm5s6%#{ofIz+&V-|N9pQ(XjR9Sd-)A&e_wp-0qdq~B+S^orK<5#*-qoF=B zuG1^`^tEx3|8rg6)m`6S6wd~F0>>T9fsY%1(w)2zpTV^!sNW|Jq=sfkzV^}n&dMdF z?&<_n&E+Sd4x2bJdO+v?9;KlA-obA$gcGR*dfGW{ zdk;E=BD{ll$Jp-48OBt<$beN9PJhf3OM^a6VwInGIYpGTve-Dk)anD@aTXmHyLD33 zVL8bRN3f}S}HKA7l&w)P9K^jHcrMY_Bbe>Y6U0v4N zru907!rFzS*g#mIBc{FLPk9h~ccL1+r1+`H=h+<-GFLkwEUz-QGkY`1OVYZYpjzp- z=C7bFz|8ms=LH?nzJ;AeFq~#V0tHaIQ%1vBaLs+ zV>qU(ACDxaPoGDk$Rt_86L^0QSgINJ>40N^lt%c#^?9BT6j!9ac+FAJ#{Rr{eK3%H zBmGmbKQ(hNEuqCgddJs`AN9-_uW!M%`i1RjZVCPesoL+YE@Yiggy}PiDOsN8Alh{O z!;MbW1SR9AoV%^lPFS1GK|C>@sl*F~%hH=+0$|5Ehf@R6m>dKs6=dB-YYE%Z4x%!U z+jVI%Is%G2g9kz)&$Miv(+-_=mBk~M8IOP#1Q9LK8Q?smX~H;ygmn(zxS5@g6g?e{ zn*3GjBh0k%SzjDh2PDg%XKTDWmQUx&)oyKZYN0*y%Jog#Sr$+abM~bzawvlam;D$Y zq~IcqIh$WOI0*Y(_L$kW-> z5R7IV2>CX%&$E0P_Y$Yv^V(0I{lRWmyYm8G`VjS(Tyf@(<_MI#Q6APFo?_OilF&v$G z*eVo=C?#X9Zc{NoJoi#~d%sx?WIrC5=B2KyjJu^mZs$Gi)>;v4!c_krh&chPn>+L< z$OQmxslV89df~AWUU~&Mh>pEXph7pM|2><~|ID5>)op9&>`!E%RUAZm#|i>Noi6yn zLy?u_;6$Ay>B+|+!g60ahC7IIlPAcy;rnpjb;*j$`)>jNc-=Hd@G7#B&UXx5{{>n+ z<)%9-2G2w~;n2~OtA#{TjA|yHS;(wn{W?{;9Q?iehL)4A5jw~){_gJd3Plj z_{58lu=is+&w3b^oLVM0)gma+UGs^AzG8H&VNB zza|wEp@QwSf&fAy?G@^lDN2aUPw)ZN^!YLmaG9=-+N1+h=Bi#OJ=Ng8ET+wwU;%+I%v4;$4gZESPGk6AsR3Bbyw zN5HP>JQ*=@+S{pgD?U?s9zG;olvX3@#!yY#gwy6=;`uiUzKVi_R6jv|DM>EKAsMcW z-J)$V*uc*TAg>=Pf}-+LgOSn9eb(-pPzwrn2%z_1HAR7Wg>wK(nQQ{qC?IB=bAw*$ z$(_1js~@VG#MUni@L9*uYogs`u~mNXdamq;7~889M*XHmZm%7; ziy#$a;`^jm%Kc%=8@)|+8N>ua{AsjQ`;~;G;>vH6P`T<&fisXjG}Q_SLMw#b70bIEYs1GHeT9 zh5_R2BML<%0vN#ftzcH=){A8s-gFw9*(g@u8-26;#T2ZO2VlSSO#Q&Z`N~c|r z7ygMrMc``~(^})%N^9LqgGD2+oA5QKeW#q;#bdv=$OO^fP3?{*OekogP0526g z@O*@jOp>@{WKJsm>(5P$Jr$a8-l?R`C*hh#W_UWhmWib(N+}x97uU!V{ zL6c(NMJCBnJzQhQKMkcUIk+PbQGc974pv7zg8Jxd$7;+Re$*oCg=Y<22R!+gLoxhu z!tgbd=JQRB-0py*JCP_tBH(PbG4X{5sOB-qAbf5SdmiFvvuKDqf#$C%naM%IB)-28 zB(>CUW~$j?^(NYFrS}^qSFHCEu1L$Ctfak_ z6ouF%>oU288a?v9Qkc0HA#%yNsq@H&Y-WqLGgOQ6pEBGBsSR(q78UAaH*?s5GVv7Z zb8~%ZHYn7m7H^nB&yg}}|B63#*Aq$Yqs(&N+CN$V3{|(>LV%Ums)qk594_VpXoSsv zM6^T2BBc!_;|nW(xe7G?6=RX!Ev+QszDNs4e@hdQ*>KP8iAi$-tx+j18o`7mWdu1qjJwN`|rS{uLW(YBR06QPlO*RW21G(JBTJ7;DVJ{_Ah;v zI)>IVFc|YhnfJUO*cO(GCJg;vIVJp-@A%QkB2?R-U0=}PxKMo}R{b_)?S$pQxY;9k z=$E4Wp(dt@_sI742M3;WB+ACjLXM&uasy{ujpHay=+&G`sz>bRE*HZB-f=P?YjSLQ zn+Cs+47r!3|A!(|dii2uQCuxiftnIxGF{U@)(4^p$ag(y_PY1@n=`Dv6!lW-{gy_U zNuxALWPlsR4J&pkP{iI}Yp_xJJ+XdG!Qz#_>G*9k??SXYuUd&cs9hlsSA%T~@ zS?y1W9yL^r{rYP+H^69-G-%wD=81_xlY7jPV^Zpdag(~G%kg7o4H%J0Tg7~m)z(Y0 zjT2$ImFzk-r#_hd(Sl7z*yRWg@JTNToFJADx#ATEy&DC-2d-iqtUC@qG#cT}qT zH5`!088UGZpDs}}|7`|??Q_@NiBo@fz=7%)0*bc51r*~asNCdwF^Hs`ixl__P6%5Q z#qlAW-ar!o*njv@b%QyaOprTf>AH*qYC!6D^jv(L-e1&2%|#sn*&QD+&1VDSt0(Jl zBR(+U*iJ%^DWzRySJRMCM_({!wUYMQu0Z9&g~f)|OrjMwiZEQhPe8`iR{S}gVC!Dr zf4_w_Nqv$Vz{`A@^pYMiLc5R_TYokFVle+|#Qlo4H1ziY&5zWskpeRWin zYy0)g&>-D4gmfby-6)7iN;imzD5Z1_AfN(*NJ^K0v~-trii9*scQZ7QI0l}xy!s;{8{r~xwDIYj7)HmtB~Fp9Q!N;YVvb{`wvCA$oay3 zlwmolkht^x#7HzJ`+ZDk$P$f!-(>~eNZ_O)Uh$^Q5R>tWCwFNxBJdg8FWNgx~q?1zeo{tX16rb!1#L08M1rO z#lYbEsQ)uKDO}ichM}78tqhMdjM*4*IC-;1bP7st@72+XLKuI*LyI`&RBep-1aygLpl1T9!-wUvBv!FX`!ONrhSgu+Aq{b@tp-!72w&&s}VW2_)d z-v0G`_&*LoCJRJVvb5h(7)WsaQ3#+u3;JZhJSKC^Vb8_753-{zO3@!{(MKhI~ ziI;;)vq_x=bfKB1OErF_SuQ=qKVP<{nDi&gvI~acuIO!)EZ|{CU%+sEOT6zkpVs@5 zj2Srf$@>!h?Ur;XaIU6*w2SW4$Ga-Qbt*42-7#8DH%al^?7vI|<%a@2t){_=6Zi^* zpSKBlS5CYoQdJn)>s+h~3a%zwJ{BT!C>>(D$@dbhp`w%nG!q%fjaC(5tvKt;G@K2j z4l{4qXKD0-Fo1LH-pie0}fs@6c93%cpNv4PK>&gQdc&IT5W8>uA(4!(hAS#ysJ*ShRX$toIyf41A= zyDB%Y(x9Cm5?o9U#z)Q{i5;DCM}%%DquhU2DMd@nA52zU<&L>i)!kAB&T=aaM$5-T+Ii2i*Rw#7Bs z7`Kf||Fg*VTTRN;D8P~Yn*9yVH%f!d!MJt%TytY;H)7>8b zUWZ`P7Dpl&sMzj+wU{N{{IXe33mI|9(WclP@4pEpS*Fg*TcQNLp2eG|vVf{Pw<1mQ zJ`F<9GV4&kll{v&Qo8CUD^li1@6}nD4Gs$t1JqQjEAq~bu*HwvCVd+(8CIzg+pq#p z{dVq}wt?O};w%zJ2hDhvDdheR!2=w> znr?Kx3+YP(CF+*Dth(w662TE+G8_+8!omWIbS8}E%c!a%!v7_AY07}U{rx=qm0dbr zIt#9M4xJ`dwW>z&kOBBC&ZCe#&5jZV<>klV+Nil8hp}SNqL&7uRa#gfHhw_TGq2x0 zV_!lXb_N|@Q2*g-&y{LVSF#dbU@>9u`ye&;8GcFlhzIYfvcz`@tTHprYQDOKblhdo zyM}$I>1W4+hLZ~@%bw9E&~_Opq&llD3knWYO*3KzYDWJ3?wzNqL`1#A@%(^?pramH z&lvdp9^OAaY`}Y*1*9SxsC>1IO4hTiVqCQVL3SLrOI zX>h|t;N0_0e=AmmOgp*^&h42zP2dL~zG|+#5_j!)-W_TJfumv0o1~hne|iESIWZ;UqN@7JC)o9Qu`^a{L^3D#}M&FnI zv_LZl_#uM@CaLRtpptOeC>zPk9Xxr{1I@F0TA#~_74xte;KLDq7QA3zkDw*krs*W{ z_G~G#1k}%hv)fOV=doV0`n|tqTd64-+W(l|>#o08jNE?HW!q3tQT^du1Eh?f*s{G^ z4xv0%8S-R>lQCHmtsFV0WPMq5BUwYwWrT#Ri_Gm!(psw7nj|wTjumd1l~}15OI}&y z3}xavk4k>8BabG$Ab;J4^iOD#lh{)*VOpuTi(Z$lmO{;LqvTkLTafow5UQKMoL++p z;uEFyC22PsR-OsP7`3f)f9{tA(v5<YZPSq2HnOtONA5}DK_sYp;GK}IAP6h9k`1r;bTz3n_N5yj&%?mB( zrs0k9ev0}TC{K4n5Y2uTJn2g;vd2M*6<87fPf^E606TMd8s^mtbtZZac+g^)(QJr4 zkrFzA^gTo-zBdOJ%j}m)aB8Dc@YAB?%0N@*0BDX|s7oO;yG<5WQF7dEUO8E?W1%IbT@lmx|IN_6J z3=E&AOX3p-eY4*!G_>YT!gN|)ICl)D*yY{Km3r>(HgUh+L^0vyYKdBqW=y@{NmoYN zb4)?~2EGM&#u>r8gHXVnq>A5221N6z#<~oj9a1n#OW6O2{*fsiYXL<6D)7p|Vzwsm zZeRg^2V89C-v$)r{0oWjai12dwExwaov(>INzPsi=;?YujD6H@^i*Dub?{q7mn~P` zSmUK!W^D5VkJQehl@2(Y&tR+yf9$OVM@1EeOMS!gueh|$JuBwb(FZ9GS zrcLtrgV834i^1>@-v~epx!7&;J*h=>-skT3en*#)N9++d$Glqijo^a7kFqZBdJqdvyWSU zmu2HHi;FOg`{>|@%!K#uq|g`M-61d6IB5vyy6v5`9$)0l1Q+$?_B{I+O8$|%2tTDX zMm1lb@1{f@*?v1yl~wacCq>_6_x>JjBOL^J<<0i^eay!Tjqj#NzU5=%p0DU;c-_9I zn1fY8o>mR`ah1(q)~#obFr7LHXn&q~??$BUNWa6pVq*A*So}T-umx*ygl?_C4B>?e zXgC$w?B?au{~sgB9>VVl!aK zd{zrUw^)SW$#F1x?adoMbtYuJ>lc2=nTpF3*Q3HH0OhDCuG`?+8s{vGeY2oI^6CB1 zv^U;-IjOY1q5gZA20JBcs3dGZ9-cP&?sB9=Y==~BwM^{eO{X-^LraB7`#zM7`C!D^_{JJNmdEfv)_wm{PuPptY~dY z8Onmpcnm(CN;9EB&d!5G`Bo!1%NNlu=g*>M5naFgs zl@a>Q_0m!#t*Ci8tu&8cFC$W7q2!Oxj}z0RqA7#N$EDWyjr~@f%rD9DT>cD%G`aZ8 z9?*UC@o^`wqY9GxZkLfwcah@C5}*p2k@OL-io+>IaYlnQt3g1VqAJL?+AtyhXznxd zS2}!TKh;G}yOBske5x7@E*JYuxRY9dt4g+Rf!@d;^wtkGcUrM6jDs#kz4~8J`wqSc z(Eimpv~`i(eU}E_`fww}V4Sj{%%S6FuF3V1o81>?@ch! zz2ur$iuTdd(Yd=~uxBE`UHQb4V(w|HDW`+0%SkmCG~=Cku4@nnjJ z`j311_8&WCWAhpR^3jhWCTTsNOG@mdP027}9} zz42k@B%O|tT9FbQ(XNm4I=xW8r>aY-gGepf?H9V@PxcgLtXnhK%zq%4tm&Rq1f?F&Eqb^=C7rlAtwYBl>&TkGn8X5f-A6XS`^GNf)x(C<1igc zEpYUTGHk-)T>i+1_5h7yo2f6q3t~T z>o_mc?%KMRW#ueTQ~YRf@OW$ako|^6wx62<gCn^^zQk( zn@e*oerQF(jiP>0NXfTT@^fZ#Hhx5U;s0udYc_$9FHVVMUH{j0a(N^ODlbt^8nK8Q_~w-pVC=mTpb6LTc2I+#^~u90zxlf^Q2q6v*G6< z`Fp%aUm^3TOJ{{IX0Im1rNSUsMyzcOv{=*G=TEIXr`9y5?xq8T+_Pq?l@ITyypO@y zQpvE)*u5;XW%(v5M`wd;V=S{zh09R z^A|h>idaFAnD|l^ImF11uR#|op$ZjILHwXa;~Gd3jpuK^x)($o$M9x#=(Dn|)Jsa~ zNFagRFJu;j2~Fv5uOy4vp6ii%neY7elDG5yRtUS=V}Zf_T5h+pZsX@=yBQ7Vv6tSn zE_gOq1MYJ?+*j-1#xN{Z}a{#|iBcBRj`&#;h$0BjAT=vi+4)H(<%r*gt$Je_V-y zb=K!tu1j0+j!>uVvof4B&F>;#3iu11L|a-~OoiK>n$0))Zb>gCjW9u&)Y*^bc&i23zO(a87NNA@RraR+8_j@ z9gB7Ujpj8u!O;=keuNsxKW(Si9k=#N8z@y4*8~){l>-1xOI>`ZR0~0D?iAf)crb;R zHTloOGr-20QtlgTSO#J~;6CFqY+^AMp1=H_=MU7WwxmNlko9q#|6w3~y3PTSa^&?qSgzfsD{# znVFq;;zZRTBIWh-SF79l{`RA>h6H2Tz{zKoMs*vFJnXMdk4J|%g zM~m!D_1@(~bvoCBqkHO2GBK&h#L{5(US%kT;F^KB`mF`m zw{9wbl(^LmrGX~rN(?B4&CQ_gU7eBQG|X`A8xIt}-W$4TOj)w#ODh75opV`@9I_x6 zInEQzt!PZ>dhYQM$LSW&p>q&wVY33qN%HmnO@KC;!{Y@P_7^XvD)Q-ocJCor-RT8{ zisNmbXF}qP1`PK-y)z1uu!FZ)3iiQMOBol*$sT;9XDP@A4>iB_O+y_gTOD z1O!%v+BHt)`-MXP?klaJ!1e}5@X2nZq#e%X*Ul39ZZF)M<0g-ugE`p^G1p`QV1`%O z8%YnrxLeze+6@Lbo+h~vSc3~Q3EYt^Zu z)z(%3*!n$zbe89iHb}qxPX8yMO#%0lv z(r7CcP^*N+ghfBKJ3an2HVBA;On|iRFy0Jk-DB1?9Z73x{$_Bm^^8J8dbeTAaRq;n zKw!1A;>YQV7|ucOfLmlYASLVbZvr2pSCl%_@MjjO@#dv{TC`@(L@I<T zRL?H{*DpQuT~m(oe_$J+3*-U0I7dfEOP{dLWc*RbV_iC{kEo9{EPq_{EyqxV?%j9N z3q}!>AJ0CZNT(tAv~2ztztajax|l-gn1R=OrR?Y#fdz*->`ZFF$Ke}~aZ|t1j`gb- z!$UM=m%F~cpPI~@Txv}j+ddH{_dsl93Y(pXL@1^*vGbJ6&Km)%TI(t6m#)kQVatHO z+}i+@5r;6E>0jP)0vdI_IPA$$p74&2YvjD6>>EHy@*3Y>5H;roc3eiBsEn1qjKq77 z_xR>>yYhu}u{)uSmHS`^r8y6n;HLP7Y*o*BJ@z{(Fn<&YmtGzaosR*=j)rmk<_>9T zF1?&T!2dqc7wR~|It=ivx7;lw+@C;}K3pGBjeh;sK|wsdv`kq%UBDssEAG7((jgh9 zdbi0jn(dmM(IJ`@NWAJS&~_@j7r>2W;?l#4+L!4bH+FnYe$8uK6$3=Xjj9{)aSqjK z(7Ch-5PhYbI^PV=M~zmkt-kNNjUf8(fyi@(v*0vXHu0%awM(3T;2`p6y*N%wFZKAj zJnu1(Z045sJ|(N{G_Q9nk^;Qp!*+(7VF$ZjLn?+i5cxMOUq?QYqUbT<_LXW!ho^}* zBVwIiO$NjKN8f8DD!$3^2!#;E*3*w>jhgJd0{yZJ7)?C8N#_s;{io1gmfmvEZKVH| zbx%&WGTrP;yazHod&EEJKhchl5yYI8R>>pU{D4LxSWuvyMxJ4<*&|wk{i)raa|VA< z2<5*=k{GoCCZz;gb)mv+IA{mgI|hTe8%1UN)dUNLP0haY^No9n~%qedCM4}=Amh9v{hlDDq8K2^s; z_JY5G;Nm0@bXcyOF#BTG=-pE|VcYX5#jWCe?P5)G{``~F`SKSRpsUpjw4c~f5-bZs zh0r*$vn4v!5Bq@Bnr;Lth4;+>-c)J_gH&NUZcfVndCNa&hL8~H;#g2){`IQ+x(v~G zYjtFPhA-_-sCRLPg@_%>LnE@ zm@{gEf^QzuzGex98`+-FvNW0}msxl90yS?w;PG2~(she$zBN^s+z|%oS0q?J`@@g3 z*P$lIexx}kdoV*v&F$;X%#e-yo)AN3@Wn9V^(>X+oZ_1k&nA72gP>iuo_PJ}p~Y{# zCux!5m_9@86KriL>Y-!9L5LiaKY$;4fP0}s(dFWV`XKf3vh$MGa;1!aC>^`=v{v7+dQUc{0Nx5 z?f~@hIAaD?bfA6$#85QRIDn|W@YK_GNNBcJ=7WWMl#*fwxF9+-hh@ijuohmGfr1=| z{2T+Ho()$X67sBcc&KJF!^RNXD_j*ONbFSoyUhxb;Q>pC+E9YmVrWI}g6}kN=N(vt zE|06hqJJBuyGd?Ddp5@)^gO(BJw%&f{Dx3!AEEoKjZ(I1qxB+Od z)1W;#rTeZR**=(m!Gf$9O1xh16`sx(HJlrLU%KDzF%YkxFWqGjYMcrv;T?4&tya`z zH#ES{)RO$%$@&ZL*~+iNZ;7t;NBjMuD+8X6y&urjW2%g+V<#vZ_)^>ok5b^^o|B#3 zUQ9GEgWa-zBKjtFUf5Nqmj`_-NVVCIA-GCv%nMU!yE0UncZUH^u=Rr_ z+*QWJUkdC;jf*G?^0_&w^ne#-Px}T?@cS?*O&9^MjJ)Qs7Nk$~kB{lH#hw5DT&L+z zRv>j^KT;{Iw@zQ~qL@jCs`?l#-<7Q#XOOn4c=GAevhYOEfnDt@QN{X?4_F1{uDrqG z*gOj$k&=GDnLJ#t_0}*fbqWxEIi-SP>w8n}f-WxSQ8^oP(l|M^&$ng@hh)JBOA==v zfS&&bH)l{n5m6cv0}4)AQA~2t;oSCn_o2}?X+VoYI#r4LBt-vM`5Z!rpl=8EXer0L zv&LS>>%`xv!ym&ka9Xe#B@GI&BNe zxDMKJN1%OH!r*$*q1U9`$4bU#usrB-NG7;x=#1F^+x8_CVNP)?LJ`;HEOEH zd0NVq+9is;J|?-IPY(p}_Zbr3{;@6beYP`F>Cz|b&_mYf@mW0HDh0dnI9FQyiC(c8 zKqq8;cGq0azi__$Um9Z}3Iv&-sPyCe&B0u-4G;aH8g12(w?*H7-A;1~SjGh6knvxl zGfqJZ`_sT|(1>AG_mJ39NQVuPvNLWgBYdH`UZ5?z;#KBBl1BrZQBQiMuPBapyMn!m z%ld(a$Ec#40?nq$eE9N=?*?dLUS;l^siM#HNUjo}!=04-6Y}oA3c(sz++zs{+SwUPY zUujeIw`dMkWt+c$slX?Nqm*6`TM@v2O@(it3)Y~t@ZK=SG=prf;KU!t^EcRBsdOH zFm37y0!fu?fWW1h^S`}L*RuXl?RD~U@>Ti3_V{lDTJKtd73o{6)C@-v*LnRJlZQQ$?MqZmpsVnD9DUrQ>C zDv?erR-fFT{K?gCX@iSbX@y2+0wx%5$|nCDi!ENr-kwJ$`<_l70#$3JXm%l{04r?J z+X^0MKH739tBj+#zAz5<6Ry1k3kwg>rAyxcsom4Yy5-@^ntbQi=r=mi=mHJ`o?R?{ z$75M~A+l~Ce2ZTEQ}n7XbR>XTNu_P#h04eG({Dykcl%GY!Xqr7sOz zviqFpJ+PleE#p)3dlhmb)| zo5peT?X_b-bOFlUzKh=7;PDs(*eOk{K-r^R?T5<7biuv*FP5ZF|5)dM`+w?Lb$IWC zr#y_4q$N^{U}&vlab%-NhS=_Ov3#g}xO5?n&*z2L_^Mw8wv{HkWA=PSw&n3G{(R|EmHj9P zSZ`MbVDO$wtrLR<!T+VUc$3cUYru)W9I zd@)w?D$a|+v1;;pl0uke9T~R8cFRjRsVfhOCt=AL*8{;$iziiYfmC!+q@uHPLODF4 zU}fJG4SKfSaIsl{5vpT3G4FgGS&n<*%{Kq#1N!`Vkx@l-^}PS|-0ybh157@T7ddJ2 z!11OVw02SQt>6#;Zf$>f2>#Bo`;q_7kvC8&MZK^zuo$Wc@Ug>wL#un}>CF?vVuc#( zz}0Q4VlqY@Wnw-90*1?CFl8@LK4A3N3js?V-G9ogS3v7mt#|5yQiaDMyGHHAR?c*t z4Edc3i6%DA0eyFLy|aC3e55$`d2jh=Al05NhnTk0L8RuY7-(BC@6~Z>ZQ<4ke7L*r z&N_&GfcGacTb}&(BDEw(148;XoQY$aSm0Abn4(s=Aadb858Q$s0Z+h#W+t2LXO9(> zHifp$N$*o2pEt^I>#$0NPNygmN{oU{ey`W<Z`Ic+Z42u%vzeRiyGLI4q25-pw47gEu&!s_Hrw`)}Wke-4Q^*efNxwJAN*Ct;R6}w-b zYF_9hn%~(t9=y(p%S}YjT{pZi)2*PRv{OcAYGIdCux5sz@J6%D4S5i9eHQ>&VUsO&g0z_e9*zj2$uuSr%#k~c{ zxwaATy8loGR(WlT+s!I~pN&~G+9sISL*+%B$5tm6;2uXq4|V-Q z*!Wp@^{)K{QoT5H;nnS^jz(p%OVin3JlYcgIaT2L7;K?*a>pA9;4K5{0Ex;5^t;+0 z{Zc^~Ti~F_d8X|#t9YWoivF)6kH?EmllUcN*aI?AFGTB%`4%4Q?g-xu`lieSEd~O*`n%}~eU`(htgKfY{LVhz~*|?40 zVWj49^cy*0@+LwqU)FYz?sE3MkUW9Zz8tln)w_tmH*#*XuKE#!6ZcrtNj+V2gOGo` zwv5(WT$Iy7U$Y$^k;+{kYV~~q7-H`ydiRQt_8ceICcS4lCCA>qSZY6Nr<|!7Z%|`1 zy{o6|_0K#^BT$Ljx}Bbd!aeM+5r%q z#b6M&k3RYZE&PC%+J9inSzsw$K1g8=T~q>8zhzi|+O(C?W~N%)Wn%OJVhTU8k6wY& zP|$I4wvcQ6xzF?@IwR^L_=%2+_cr>?RGG^ZPOc3>3lB>gqg{(sf2MJiRDlRVVRqTI zRuP*C!At1?9D_e-ZL>ZN6JnC+^X0&cM77iXM$h;)E5M+el08=w=hl79GtuAL8y9ab zdPA469h^r;@D7{_Z?-d{l=o@I#jsO&WpEx=9yGm7kaEa&lb?G*&jkHRq-HtzFjR-_J+1O9za zFc1y6-VLE+O-R2eDZCF~-g*WD-zcs~NY(F8A|J1|I?X8SnVS#SbEY#*c2z9|p=r5V zSacWH07#z37zT`QgE@~S{BzKB<#k8Wyv140r8pORLH)%Y|7Awd2dQ037foIY7*OrY zGz{?yK7^02I8h&+23BwA&kz3&Dm8ML)JA0qevQiIRtKz$UqfMG#9YasTg2V7xX;fH z+GO#Am}8)c#X2i(V=fNhcWd=o$IWv?KxEo`iHaald!kgx9Usg0mW9Vb41TmtRNoi_ zdhY_&x?twC3@MyvN!`{k%&?a!FCNU+0Z$lh0`X;{tqbnWhD7rXqWz@qEH^JuYTgdb zCAG{yExKJA(of5bNz}|Fq`teUs2W1Xm3>c7cvj_+>u<&!wb~hNZG{~BmjZerY{Bcy z!o++JVxl*!ITXJGX{ou$@iN)9a-{fWa{17eU&AM0czbHJ*En=uxMn!yC7XrO>9+)3 zty;^Aw5VPjFRvmD5^m@%YPjeW>M-IX9gJ{>A1f|u5?NZlle$RU4{-`tLSPdGG+=8I zc84i{H`5|i8Do#9xxRnoTVNge2<@7U?u|fv*WM#B1!+dQ3WfL_I1$a=I2Ath7S$kn z1)`IFQOcgNC#NCUEDW%-AW63o1H1XU#9)0DtydfeTf`Rlm$w}_aYg`dD^sW%IWV`|I&0rdOBd1-d7E&*r)Ej8_e z>sfHvhQd8pL6-fLzaqqpFeSzm;+FZBo6wDecllV^U2oJ@wZt}{DxvZWgW_lp?{t@k zO$>PCVb#d@_)>2|cgw7Le%`hC;|O4LLe8kO-BK(iMiC-`X*=&ZGH<`DQ8|`MZ|d;k zpHxP+{VAWPf}ZV>euB)gk{XB)Tc8m7PjbycDU1${JHNh7%hfnQu!T1@QiODgsP2CW zzj&E|+|f&De%3_WePwgUO@&I*rcgG89ra$J%u#0G&8n2FJ(6Z@Ott;-sq+z`oW$y_ z2jSxJk{(uzSU#4=Z3Xxh$!Q#p4`T~i1-tS7_*q)fS=^W6P*`6g59jjQi1x<@lu5%s9wZ?IR#^v$-0IeG|tpKPu4Dq@*k z|Eb%`p=uZQIFWDvV)xJ&z4-x8fJ(!*@qLH<1MKdsy%pM?s`n7^PCqCkyyX6-$@6&0 zNh97|sGQU}&39>fAwHC>fCs(K$9i{3rVyX|@7FH{E1eGFVt6aLnHUMF`Ao$@B1?9P z!2U@Ab;yn2ZTjFqAE1&V#UyahcE2<6==mlgo1gGG6XxfeqMw(?N@?t)X!DyHk#9X# z7o>%uF0&r%V$O|T=P9t@JFv#KJVG@0I`2(uL9`{)N-tf5w~~7O_>q2f);9D)LmyDj zy73Wmf_UruqpS0QG~(#O$+g~K9qP2+>WH7`KC=$-@J7AXX(j0vZ?(xxB=&+R-l@09 zCT(96vf46CiLsLS?AbZ{J&$DzrXuy1vRxL9gdO78G-OM>#D(;$#L#N&Kc4H>3Iwh7 z&%3jEPc$d*|{-FkG*joTkRdAp2kKCar-dMt9#@IsS`?!R4|V<82h1$ zP{aAR;)9Vck+b8SxnCBo(`u4tbe8Fu5o+mL^I=tAiXuolRA0mkj0@^?Dve4P$jg%S zroMORRsHul7pP&QXi`D<@@X^SQKOOUypn4V2?Zot>hQPdQoJApCy+gJ;!WtBbuUHZ zgO)!#V+oPp22PnB9oxs>%WX12f+nS}_dcH%O&2KpZAKuZ2`B?BpryllJYqQQJ9qnX zDPiiQp4;eUUVmxt`VYJmd5yLl;^p4>kaswb(C_T6ke+oo>&LjL8QydR4TZf*D?cIcqe%&p^XxHa2 z?h;EX_dkPQ?MFQm-YfxfVQ6S}SnXGY#;@ed(LOSTi;Q64jXn_I9gVDr5U7X${%c(I z>a*|VX~!}ARq{VUfdd?mccD@zi2Z#lJ7$XqEk$sPhGQ*C0@?ADx>2;k_p8#m@F+?` z{^C3=j{vLV*9hG%ADuvZ?AOzo_`Q27Fr!pF*ZthIrhEBE;oe@ z_y;M=P>DLu6>@ung6c_AgC>rlq2UzYlo=QSH-Jx@@%^W;KbZNW6c2lDt}lb;Y=QaL zA&@PcHt&qkS1g4X-h_B4ptmV8Ix$Du^YFE{FmQCYznfd8CA?Ac3df*1=~Abrjb``z zQ)>B7Ye=qd!UP)RFJMxRK`{1Z3+T$}eS%BVXc_;pN~)LD-x@E`G4HCd+xqr?>K;{w zOW~c1gSKM;hsWX5-dB2u#!dWBp!}(W)zWCiJBpTvh3?%2ekq1}er8_K11A!)i$>qN z4ypMDhmalOogl6fl#)O~GD2_6CoJ(%j4g?~A_bb@NpX|7#43X5rFHmbYS^&ZZ&?)% zvqr#}Nz|~($Fpg%HHm%%XlMC&W{}VbzhDG2!3jc&J0!e7U-vW0H2MQcs~s<@+7P)& zy0HpG5ArLOt4)Qer%9eb zyiGD6fn1F5H8&QlFN1#n3RZKrRnokkxJTdgit_;x!r5)VyR|oRKEpMXY{T_zRygDW zP@Oo;ZL%$G_P!Ss_r}M+#ECa?`5*g} z1}%n^)JD({+y?YXfa-f1+z*BjI7XtePVeQC$f_Qp9{>@gl{TEs|fki|v`Q~*5 z<;zjD_wy>&x=>c3 zWSrVc;gn6DFW<*Pt+j#lWwy`xA)8?&nD#tXoCIEe#h$1+5`vwVM%-ojJxY;NyFiZ@ zr3<GCmW7Equ6?7=tNMr|tC>;Ax-N;n)C$Qh+(d z=+B#gUBCga<~8YX9f0}jD>C}A1nBhqWnDRE8tMZeDn=M4m{pIzC4KX*%eqW|gcz#< z?SN=YX-;i)XWgyPS69ihXJdZh<{Vqfc#N~__vztNsF%ZcE5(L4v7`C19mEb7s; zFhe}A`~a5y#BFybzrEEVeX`sZ&#z#C_h#fVznvE_LA05wu)EpIrS^7kC@wT_Hk>5LLN%5v-?`a#=<4_(Sf(7ZqT(B?{SY zI^OkT7{Iy?l`nf3L31PgrEAGKc?1V93mv^lr>v%m=2-Ai*PbrN1)cR-*DkT<3Hs4` zLtcE@d@ zKMB|k9&cm`N>(|}JN%%FJTq-~XXx~yIt{|PaLjX4t9^m?HH1kS>CKSy z9XD{*U^{HMIy~7iT)L`)9Wb8Yf6`Y!sA<#Ta?*^_Yg=U_($R*f2<>si)RT#p&OeHV zQ1HuuM=_9oQwo9nas9E+5RCl{1~aIHOL`U_Z;uP@G#;R(vFEv=oa0g2DL|Ojs2qQX zK5@5W4%lpEk%gi@B(52?gB0vr)xWR zNdxok)tYP$0K~ug?Jvl0j1|Pk@EYrJiWSnGtBc!-~ye%zwOVB(z~DFAc40t9PSY0cU%J?YWZ${g4U{as77#`qWMa{&Q< z$HTvv0;(`uVDWxPzFAjh`X5%|crB1muX7egM9HK;NiiO*AX@#9$e8Mo$sj=(rs2ag zlfiqX*IF+SV-ZS%u71Y>#zpg;{&*g{X0O$o5SPimuE*UQosa*C*CX6(|KL|$78CmH zJWfMO)}Uc`5O576>;sum4!z+a!V7W&Y0w!(Yq)uy`kop0l5`imo zU?sp$QRZHOUeR|A&s%^?N^Ntn}~mh0z!O3b`4#UsZY2&ooG8V8ry49Hnm~zps))t zQPQtt=Hbx1M+$8qJ^QIBz=XxnwAkjZSdI3bK1R`W2_sUjLhoae$MNhg{5i8@JS7jl zu4yF8FH3 zdL;9eU3Av%i$T}Is5v`Na<62LWaZL3PB&es4o^iO1H+oZan3(=hBg$`8O)<-3eEZ8 zmvzhrtF}C$oQ28u_JbL&ZI&k#;3nTb0^mR-j*h}z#5_~!R!c}OIA5hS#|omYBub3u z#1Q_~KaZ?)e78=il?$wqZ{;d~H~agq_@G#CG@XTHl6lr=tF05S@_5yZRf3eP`YUJ_ z)x5>NdNa;?=))&&rqW?WdlI$@_?JOpOeQP7%ml*UfLaXSXbtY3A4N@h` zQE;=B0`x9rL3sQ2dK*LOl-_VkOZns=M}x=w6kF6U(N`s_oYGXe%+g z0Mm7D-XLXyK@~qA_EC$PP(g;db8H7ZMTu}rJ!lF6I{Yj4;ig`THKC(Fd%i(g=E(T= ze6#sS3f@ir=dexY`S%;`?(@Fvpa2){Nu@tXTC1pska}|XN5Q4NI0yt<+VcoinXi6D zJO#v%?MOK3A-{4MkcT#H~LbfH+r+Nal@cc)^wdP&% zcvR4%$)NjI<8|qA;WScQkGvBS9Do+M#-P~Jot1%5TbaB!fDTt7l@frI_QNBJar^n6 z*z?dSfLGdW)Wm=+LDVtz)oh(^VTk8%IbfqY$|z$$&QlBAG@txfpb7N3yRR3Ty90zv z#}XW_U*9wSct;~4pLCGeIKm&4#qy=|a%d8P@7v0$NpnwkrtLL*P=sfp!ob6;}Z?ZDi^dup7@wzkjoOJ->b_UzexHK=c7WdA@2O^7$ zzFl*c%ffA`usBTpZ68RppI8_RW|&vy7LJU=Pi~%u7g((hLA(of)P#Ni#0skbkc7OS z=3~*d2ZNk***d!XfRRSJrC)b(6kIaN`Z?-xpRvbJ`)A_45P34_QlNP~53$iR{y(O^ zIw9QXd-ab2If%A)PL~By2(l> zF2pkeoxuKKgMIA07pT7rKptQa=j0QV0Z8=PeAGGGXZB}jpDP^4Ym>$2&l}amp(dS7 zSv}ATJ^-m=gv_#Lq*`PBgCO5h1wa5LhM2aw6LhiNpsdYWseCe`m!Ug!CoXF|87M<> zzPFzU7O2WOjTRS(N+a0to0-Q!BoDW$Xem2U00BEk2Og9Wrvtmds3avTSW;5*$#^Ex zzsh5C{QkiSGZ{h&E%gTXePaUZfHLHHNk@P_mBq54lAS-UR~Ig$V> zd3ylWCqZ1A{d)>8-<2r|vysL!S%FXec}DASN_=od1BqsmT!;RxX=5Le7{8ir2`9Lo z1(9UD=CvO8jW2zsWRiBQkI1*jTRZJ91`iFGv?mT3Cm(N!7YXoCmn51`#t-D={F>v% zf-)M3!oq|Iw@G~>j0^N`T0TE<_W{Y{_Fb=8eM1?HO3fY+4QSWz0m|^}#rf>@JD?r7 z9X|$$1u==-@6c`BMNoYQyS#-z3?j3uGAPiy5`7*j|BeKCsGEU)y=#>z?OlqoXqplR z9AIXp6};U!0af;@YpNL5Vyg{wU;ZF$t_oCKf9bby<7jTpF-78v)-RXux?zaw;Olcs zCy~-V1t*VoO5=QESuZSj9HmcaM;>qs*48z9w(=&E!gs|%%pB{07u^n*OnVsZ;r-9< zmmLbHLdGQ)xhj`F`;INB&2TbFQ^M8~5-c*u$)7F)GzTR(Kt>cAV~zWQUfqxv*7+z> z+3JIm#pKovO}k*9onZ`w`b<_4AkG;8q_C}vDZs7qw>06FgB5^^?E|jGSwDzf&CM)) zZV7I%CR|Q%juhB}RL8NbNfa;IuMeCE>H z`st2?d$J0oJ|4YklFtAJ@k3Lf!H(Ww=vQEwuHd7eG|i=vKwGw46sjEVGF@8`;p+@pqmeyX$}3RJdECI2Qo>ivWO=w3uQtl&>_d-9vQV-u3l%4lHcoVzt5siZRu%x` zaSobDmOFhRpu9zm*FPwjej>F7jmk|j7m{`{!`Pg1ndUAUz&~Q{6+bHzB(ZJmYc50C z3U--Hr)`l#1xQi%37GOp>Gm7sMP2DnoV$8^jlOt%Av~?Ya&%rF`?513AIZ$^)A7kd zCg)LQO0C2TL!JIJ^y_ip7l?9o4!r>LfySzc5{Ps0%&u;*krF}|lSslE9{Aw%#+%h*ROTL@F7z*(~+>wjhqC6Tr*d%g>*(o==lBHGvlGyh@2Ato%KU!uxqa!tVe)Yw-BIDw&&yg{N*RX!Q3e*^H=zk`{ElD*_s^!TwW%=cToTjkVr^u%1o(2K7xdsa zOUPO*w*XYWA?& z6L5hQ#+gBEKMTo)Wdyt;G4vG@FHRUxRCOZly1`6bm-zyb$YZSSvg#A&u{x!I^;c!G z9iShNDS%ICcyXfA=?D=N&;}D+QxmnXgj}=36iCrIPU*&1S20p^O)vwOrW1)5wxLM{ZnXEk5q3?s(3d9pUx4{9VY@}VARUO|} z2s;M;*hKJCMf_7X&|g{w{m*l5nm#cqDKLjL+!qwWA4tA!8%p^+>3y+D&AJT1zaCiTE`ak? z<-gJz1MtBOn}Xmb4HSHs#o@5&hOC(wrN~{_Tf6jgJ(AspwXd;`WP+lnW%ETs%tWT& zy+1ZUHz=S2WF~d)C4=j)nP)W(ej5qGdlGpU^91pXdhcc6O*zH9L9AjRpdO&oDjB&a z#V^lumlfthhBDS3icT#gn{6X5r#pQ9WJO7aOq{~+@a9ajJDDZyh5;x6-vX&14>M!Z z!vLH6r79zIU$-acIs&GsPpt2AnAWD-b+A#Le7y%c`6zi7a8y9??^z7sE>fo#zZ-q)4Js z zupRqIh|E4KVEV3qc9yzv=KV^SHU?_l1{qQ?uLM`vp*KzDaR~MI@84lNlh1o4J9h%< zO>iO_z-xxXc`y-*j;63k^pp*{;5w7BR$sbJ7DWOZ*TBVk{7tXU%h}nc`|ZuzS9Ieu zppX&6QwgR!z#5pZ1iu&=F{=rvBzm0`(D;2_i@fKO;?5g}7bt`DC~f#_iiv@k+u?v+ z)Pv&k*|g`FlTEWN= zArP4^tF~&9)z+d8f_T58NMg>z@8dY?Vuz${N}USf6(AtLHAzQxg1=W5gMXm@E8eED6}R3t=KVxD%Z zhL8F(_`pS&1WNSIOsY*d0hEGEI}szDHnt4RT>;AMedFI{)?qPBNYjjfeRYLNqmW3p zby*-r)KIW0iF^hCym!{E5#1m{qmMvG1yX#eJB&orgyBJgTNV8THr z6$JZ0>cZJ1n4z^|f4aZq7YLHN&xd>gBc_b*fBj*agO9#?m=pLA3KDi4^DVS2Vmfid+e`8z zcJb|V=M{!^XlHpgXjh|^(trR1zFzN;gJ?4)K@nT6O51J?V-fy$qz0=KkLu;;zBFC! zch>z1HOy>w-LmF0E=>cKwJP8~IKT>4g1VVYuj0|Y5cgh0{f_kuf8fI$-k?%Bv4Vxk z)IW%0ko;J)$UxGDi0Mw^HYC@2o9VZHJ7lbMZ*J#zMIJvt#;$N1*NWxNFxI#bgkAP5 zlzkNiUe~87o-%juE7XlEK!R*4tU@5^DrIVA9&37%2R}tHn&Mh~yZunca+*CLno|QK zTi}6J_*7(NkWhBcA`J0^<9@Lj-It?KiDP=B=$=X$B2>tl>`$V;_JDHikhb)_&L+IhMAe_1YQh40%qpkPN`RJql___U|S434!P-J7twQ*BxipK2@QV< z%%?qz@p4SbxntT#PYFZ@p2Ld+K0MMi&k^_i``ck8E8e@%wDQT|S9{IhHSV8g^_fFt zord$t9v>8|!rOe72mz~S8v`MyIsEl*98?qMT=N4h;obG~1GusA4%pPU5k{V}`;6581K878{O?XhA@cFMl|)%8-eAVgtS4rOTe2%fQuwc>m0!&v5!$1XR`mm7z% z_T}6`k0A8Y5M!dvx_1rY{+>;l(iPR^`qW`k8h{(vB=(d!@bJyt1ov)SWHYmhCrz;; z?SVDv^@`l}ZQwrDSLE+4`8PaHKq1fr%#T&SJUbdmCJB?PrURMASb9u@$Yk5vf%FemT?sx_F0T;~g^K*Ot8Bj4m&e%m=k{b%1nLTY*!m#QQBB*Ep5!G$;B~KO2W3n{0H|k$ z9}jYDsR61j2Z&Xl%?2Kaj|siwgxx_{vBk@q46SIr$MKBgd({$EU%1NMbbt1ZizVj^X?`u$XkH0nJQX{5Dg%qW!;@suXQ`o^AgrSM;9TQy_X8I>)*h`8v=AaeT6-oLr< z=&Q@3lvr{qd9uf%ZT;arzi+CxSt`<5)9yUq&z~iobvmEg@lvbvhNekLe)XQUDA!~9 z5FTsCz<#a-k3y~1XJUwzb?ApvCr~AeWcqW*OIi4#z{xY3U#A|%x^rqNv9ozBPqT?TW3JiofB}@;ZNO{2a2q(4t-3){?cBXyVWp!YML+Qq>xrgS;5tojpXh``N2DzM|WDDWyA3{9B&lY7w9763q;%0wDOWu+o`gCv`=l zC5~7GU2ZW#K1EeB%`p|MYWQX2_7`x~I=t?#6q4h!y5(gxoj1-7A#tYZiOs9390vZ) zPf)&{+6{iOlZOZL4Pof-yJ(`EfX73hV+U7In)J@e!#Owb`F}OyO$(r|ZqLt;(^p?`Mj9 zS}di|m}O8xQo3!1>2Vp67Yft}tcKj0VblHeglOs4ag||}3fuaiZNdu}HQw`M-0J#1hA>2y%xBFl}W|vnjti znI!VG$;+>Kz~N?E2I&l%yW3fcth1Yo3VEh;?4RS4y+GGSec4eHBgC9`0%SLaOuoE6 za|}}pDw|XaS~Fl$ZXpc~e_#UBFCj2~A_F^A2Q|0~sNL??K<4#EZ2}7Iqq|4#1x!Qi zs*vdP%EFPiN3*5I+`?uDiiB*zpB9XVda)Q1v}u0FHY11aq~Fe@bYB#TorY~Mh|>jU z2DrZNVBXuWo|rK1&ke`Fa^YiMlkA|w8zJ7NR;#9X%xn@5q&nmO-};URV^MhHu0zuf zCf5RA`m;~ZN|O1Z2B7jsk5`5l8;=Lx$8_?W3mihEj+Cy8MM|*8xA*GcpMt2aL`E;Q zuv((`TKunyD|+`o-VUbMRyhIUtH-)VG{5KP5w5%PP-Ptsm@;;zyihb8DCjD-Z%AAo z4q-*Fe#D|;Du(T7@_Jb9L8XbWs@H;rt=b^pH{R7HGGAl3TRNsh1sOzhXp*JxBfUvN6rCrt_xtD^6qQ&y z5NIq01LRY~L(413#(0KEV`Kf;OF`IigMRP@=0JSA zJkuvpn^a9D!_s`gggWK(AlM|vp}ctrEkZ{ejT~EcXQ|?J(=PSC2A+^xb)l6p)GPf& zD)CHPW7zeML#0IPP_KMJzIO=p@5z9#F=Ui@Ct=F!7I3Q^oFsynQ;a}lp?%Q-5@q)W zeM=(x9p77>cR?R7PK-RrzELv^fMKO%p)^>6UaB@8sK|x{IKP(3WH4ZghD}H`BEQ=I zSboel82-}gDD(*1Ls><>n}Q?-?|GAloQ*MFI3kA)maH59L0IMNXgzP*b)Y7EaPx3; zLZ54LR7RlFf$%J1oy$#lXW0T|DSk%v6g%`~JQ?nk!G?W0*!*3QtDAjyA0(JK_WQI| zh79-ujg&^^BTqeaDEC4Z#w$k0z)*)(me4~GqA(L9H1!vS7m8DzKh8E)7`GG&JToVC zFHWIUa`I500cC2*cX9zIW!nsGHa>G7Ww_flX`gMfh+v3M%<&*mZUn3+8|Vufs4{Nq zlI#*{+_~WZC(p*EVTAN5C9^;DglrDBPf>fR@<@k|m07)ak!d49vgDb>IhX?goiqu_hFs_5X<2Nrn^c){~VRB2wDnA3zshA;( z?|2`uO;)>#KZxrDVHRf8Eo8fapk0YuYWW3;c%!$K?64>{O}31PIv8pU^ly%9v>F5^ zoB*M~;-y992X+ly7-@K!Dumzzz@kAnjnTt2U^IZ@5?_-}*sYdR_C~SXEGwKwrY2*U z4rUaa7=_;}#E<7rV&VruG}htH?5y0Ec7AgS$-8p$Kh?{S@@1OTPbu7RLR(*vfwe82 z!Gy+mW1&1-2wobIpc0bC?jq4nN(n{C#VMRrE2tR?zm0>_*!Xv zFSB{ulE~*=_Moj}dYkG}t3FZXTok<$brb$d@}IDpg%mWSAyK<2|G@5GIC(B+1F_%1 z30#FHi`P+BXi{(90KH=ZxX+##R%m*IL~4x4cb(tdF4?>`#9(1Om%xE(b1>XC{(T(` z)9SdL%lh9Z_5xbEzJQuF`dUT-7xW!rdML$e6)=AcmOFR;#0mS<7l{T)47?l%2Aoqo zG>>3cU7yS=gqa^5Wwkp2>{nVVl*)#aOid3i_@TET{S%I-2U&-wNI^&Cho?H}(v

    TiWVwZ;vGYJZXSvQ`7GEH$hBt*i__$Spm)MXa&P6T1);P)a}L-}+2d5tCGh4ByW&^y1o##AOhZ20u@}na}s=%?Tmqz5moe3;(Hs+Mo2h015Ei zVr4SA#Ykp)3d`qHFD`Qs%VfL@wJt6zLMrHe?D&1SY! zC$U=2OQvQC;A4*K3c!s07Wv3TO6yXbWQO3-BSAIOA2Zkbx$8P+Fl}lX~wy z%XqFd$bxU?pWa*SOkKc}Rw=~IYIQ4{sD93TgnEH&7zWPKz3j--$6l-HnSLZfAN(#B zUQjQRuip94k@hO^A6mxjrp@kX2Vfoh=1^=*1W22+gkXQOZ56oLGiE3(R2`|AK3Qmf zcx!LBl3~jbXi@#^Z#ygx%350 zD8^cJIN;U;jo&ti3;t33$~Lce?{9N?@SZXRTAE@pWsqV%%-blwJpJ)5WcCtd;wYaT zZE#`4(W~2^6==@OdtzClmVUe+i`v~?7Al(m%n&C~zZ=R={B)-N!Pl=}bDk~r&>1pa z92o)asXr!8>5vO~vD*qGd02!JUc^8nIUxV!emtVg2LP-ak`oz%)}a}x*RY1C%@lzh zv<}IeGMyk(py|o(HSoq`N&`B`N(xB3L>bj7ch zL!LP9&NcT$$IsR|dT{{4U)vjrfqEyZUB3|A(k(5M#%DOq;%$j5R0zWf-=rqMd>~)U z2m0lpjRd-eCyn3tw#1H5?=qmoTx^Qv&-=KJb4K{5)s7%o{6~;5cziY~ibB>)d9hmk zvDa(T?K>N~;?_ZD>?nxwZX2sy(7Nn~WzeazwCj-@whnUD86dJZfxa>U!#5NCohywH zn%On_v;fAq4m(k)i0e9{ts;e88;;)Le5zy%laB#Rt|5soFO?N5tFG=f$Y>b;18xDO zA1(CNKthsY^jYfzn)_-W6pZBXW+YG#H43Zw0GKezpFZLogvKZlGs_k)*3N|zV2(qr zCUj};HYzJsaLPL5Be0~?{c#8N+dqK>AZG;fGUz78BdTr{#G_8n9gWcX!Ac*S9XqvX z+0!RIjEMcE7e#9LtbdNZtPX$0HMqA3W+Zhf+&2E4Ml(Sp?i{W^de46#vDsz`)YuHE zQIDeesP5=U9KkOyA5Yg;UNdRmP|&`igwUpc38!06zZK1?otFPAo<`_yeB~3NJK7vR z6~ts}xZu9Xxzf*J+6@KY;qqAG9XnuVM9}^qKA*M#D_S6k8qoK-8=uWLWdnipLQI8% zFd6WvyK*j|T;Y1aL5_rYr0%W*FA=Yf(CIm}4jJ^`s-j0&CE!3Y>0;V}BmoMA*MzbL z%V$FMYVFqSR>{vvIbplC!JygxHX2TCk*}(?o(b9mi~ts69_nciC-lh$K;#bVx@0}l`e!u-O?SCI1h%*ath~1X8s)&$ zcZ&A=EHh~R)0;d|@mz42UchJ*mam@|_iT%vSL;%s@#OBEr{h`DM6V;1A^j&=I=d5j zcEqq{l1H^Phx16-&iSckT%cu)cg@ku6yXS$&R;JkO=q_QE<29&00dB1oTw&3K%k9t zsa8Fn(rk>ZE4==YEShI4xYSWVJoK*&vjC4fnumj840zlIrqRP#jxOGZYK+Uj9|EZBWwj%aW1w!Ei z$6`c!c=(Zs5~<>{i#BdxI(a~9ts|OM&6|P(k&)igGNac3tUDn(?Lv(q9iIat6j@Gp z&?kHTG}mVQ+v5Ziq-e>^`Gi~Obt@aT7on9|$Lo0hbP(;Ou^8fxjUVc4Lxx2m5tV%) zbSM|HhRSnY#yn>bb_dAfLLdJPtID9qI+<>C53U~7IBuf;v^b_huqUp z)*?KV_Ao@J#y~nh%$(q4xX4^b&ZXn6Mo(<&UKt@EcMHNmM>p(i6oSmf==*om=PIQ! zZ0{FbtLmM5jIQ`jk^C8PAN=e5^~(7upT2kD@PjZE_Po0d-v6q2KE8*Y+DXOV7-(@R zMc|+R^%JH4`iTnvviWh=GXG;2=lZt-`~D)C$AOW=jE9cfHTFNKkiU9+@2LC1iTZ$r zhM#lR4#fB91D>#CW((m+AnQwW)j;gxv`xJ>a479&=1B7R$2&g#Gr|qKY|!2NxdW{va}riFDq`_Fm$~R8<8!E1tcSol;l0=65%C zia^@}Duh1m6eP=P6gq!9Nq1Z5C-IKvi@B!BJF4#FYkV{8KIv7n6na~HzN&p6Ky5&9 z3WRpYziZZLk5KhzM1r4~=9Yh1(_*weQbW@K&z#EV;Raj1l6Ig_%pt9+dixf?8 zT{rU>P=@IoyCWgQ)MmU2`uEZ~oKSMr7J&lXnQ8 z2ncMt#Ux(dpc!sI*C6M)J#5v^a3EikQL2H{CZ!2s}wH*SUe#+f*E#F0VG6Q z(d7^#a(8nOJAKD@C7lZhD&KrkjZZ1gk!Z;xIakPfQG$DN7~uv=n3||uW8d*EtJdqu zd`o%u`Ojmdl5}54QQ|i_cNr(nR!#VyEE`rlaODr|#&SoiKIX!DU z0A)(Ip|N@HjT^H;=gy56ZsY(?`qVz3OUU&_d zq{pw#t)jbS7AWg4|4wS?d<(ktdkn@kH>N|iorTg7J})7h>E2!qBKZ!D-z~M%843c7 zmZAvH=*3+NrwLnG83*7K%mjOrk5R5ss?FPHd)YGa14QVTh+^S?pm@@PmCeZ|_wgC-e z!Q?XsHBZARiQz?)@yNf)@8a8oPqML{5%54Xl9fi_?0rJQr*W3Q@||&0ryu8bn`+ps zj@T5erU3aaE6dD(+W*Eu&g4?p7f@dm0b_vSd3?!FLVri=^*asnUaE&yJ4P|j!e$%v zay8amS-lj1IK|v0HA+*G3;$!yRGK(2hoeQnh3tc4-y;hA?dPh>|C!vY5gZ9js5cM1 z*A^x=2k%v#@}E88nqP&E|JpDDCTV5vh9Aus5#AM=rjRkA%hrztwWTJQybzt4no2*K zHR@tk6mbF6YkBpo!5A~96&h96k=Y#%vd8i#f1@6xOL^u1W~nFg8nD7@FUftc=g=si zNtPGmHvD2?4u<*MIYMQMIA~{-IAnFB=9!r*SRl6P>i&R#KTsgH6+eS1%J?iltad4h z#vcs@S#Qp=D!NO8Slc#!P?WP9*= zPVQTiq>omY#JF2+ZK|Hmu7L%4)BNkZP3u2*N}6A@B;&uiT6!DowdUat#(Z@ZGhsFPS&^Obwn4 z=VwFJ!Bt+6F^CAF)1cye$N@+sQkwlOxXKJ5Zzt_<+S3u6t^Y-wAR^ zlLh&?l~l)>vp$Ql>w&?*_Eu5{0{w!Sw0EDTPbf)A{Ee~N1Uy371!zS=z3#Y7mOrbH@mR{M?7^*!d=X|a*pp3yF2yc zc0S5Se;;+PAqoHqX%2W^zfsZT(q3T71xHr5hM0RrJ5}F zzAR9h1GG{p{R!}DUb^hQLeRh$vT->T$vQM^)q`iPOcQt}Ci8whn`eVOq~EMPvHQ@| z?Sc}y{39d*%Nh!K{~TOJL-s0g14*U=x{9P>} zGDJ{mmn$xVTOTB1Z)OR7eZHdIObh|OcsOjgD$<>aS^S~3b^i`1hhqK#-_K^sL$i#J zdP`QGsF_T`Zc@dK-+bPTO?@lyeNm6PL04`2D4r&7ij_iJiAfT@E*SHaa%mb9;z4Uh2tKs>0wG=TK%kOY@d?HVNM z9(<$cA>5yOLH#%q^`VWL$4IL)0#Xx2iAshDc0Gu7Km4gv*O{q@)$)Qo!C8xc{@#v? zE-ucR)uD`_sWk1kt!*fsi0)RgX{83K>nG5^d$qrn{q>7#(hcVg{|%-2m=E@#4MC7# zgh9Ii3Yzh69V}M_UG`>~{f`;8*C2$Yc5{#@7x#r`wK@_#d5koC7^W%%{N(ifoT zDZhE_4)2vY&}x9>A@l9gNz7Vv?bn$F*KcM?F8NCYT>0vXc8pBJ4*KLQzxVV3V^ayS zzyk1cD)znSd-!>n$JoL*7f4EUfeXgWbHq>>R9oeKGc0|FJD6lnIKOtXN_k=UIkS|9TCD1$Y>3$| zknk81`#cp1Z^OqLqGlQbp2vrzwy%`}pk1KKTLQP}I{*i*id~7=U*gHI!rVa*r9-n! zmNv5W&LpuF23a5XO|kvzr;)8MN{k!vTy<93ZmP_kRn-e5il1)0;AL2WnIMR7W><~F z-n+>>%bGIt`fabF@M3N;i=`HLu@pVSPQmuHBL&+t4*7H)N_#lSb*lSo3dp+O%H8tP zA&W`^xNU&Axz}36AXZD@5{Tr8Ht~BO$tXg=1hktuxUgN5ozaq1x@t^|cGMqPeJVC) zmJol=kk*#qp#3hC>xO~AVR5l-v8D+eHMR@b2)n5oTO4Ydj#zhwHQ*k?mnL--eDM(A z9COE!Q=l-!r03(y%qvt|IjTjFF!R@W@4N##K}XphU_UZHov=RK0ZcK9gfTQplOo?^ z;G?pQOp@r(vK-tQ6mG7U5r(@3$2+r@vL|y`k7pYjMx(@V?o8Y0(Xl{2(XT%LyEFUc zQ*$U~=ox07fnMbkL$UQ#>yN;-%|Q;;l40$bYHSUm!X zgG{dR>za|y-I+?q)2_*hezqto`x(@|q)4!Qp#Qt3D#sQ8_t@Ocl&3tS7{xN8dUibb z9Anl77S3D%z|0%zyxIrd(SlHpJ(pE$+TRykTFES*RAR|#4(79Gi;M-9 zXcZy1cpR=4b&&jW*Si%S8&mfzal)_&V|MT2UTW93t(!-svzfd`mE&3*s9@UUC5p)G z*)D*_vSAXO{bnMC$5W-eI@d>v&OtZrI+*j4N^#f?pDYR|M7V=NumA#)ho~zl7})nX zdcwcFz+|fK|DUuwor{qg;|UPO6c6DL&WxyDb8 zs$T{IE8p7hn0*NdQ>@h`{jj$Snj+Nkzotlk9XfB@LAQ<3$`W(E(;$Z16yTx@aQ`v} zls0YnU4K_6khucSn2pyO&hpLH$`SSMwKd5#=&dGiOCM>E`;mSS-L9w2$FRS8QGUy* z(LW<@2CBqk)A@W`#Iik<-l_l85vV&T4{)JdSgM4HuqNu)Kz7=7vbVSd!qzD$#G$G_ zK;^ubyVwn58ps0*LGGr8N`~HwiV8721F5;mcqO2fNz6;h zriSH7h9i{1P(uB-True;zWcvk%wjAZ0QKzx#EXL4G`za@hmfD%B{*(0e}>G{4k0o0ar#{VlG=6+sa}+xe?jq0K{uXx(QvOSu0@KDI zQ3~c*>$EgmJR_wX>80Bo?3r{0NNh`_5wavyG)idq;en02%c{H9m>W3D2Xy6@1n;YtOX2hWn~WD zsF7Ui_UF3D8o9~q91LA$=A0`3$~77>?HP+DV|9B9tj%U5#5*s?+^I+r!#&h4^~vQB zsKPbb@CJa4d&YDUa5DOR)OiJ?jAwl_PuNg9)HYHqGp!(mA_oYfeWg&SzSjVE>#(QR z%1Av3MM$tf7B36MisIZ?&H)rKZhLoKMVE@{bgqviCn1FT)uh+d6c1$&4R461ARb7v ze*m(tfJvbR<5iY$4Ce>^yr>GVJ0bXe0Tg;@q5j! z=S+Rm@C>5j7u0RqbI1!IZJe-IPP^m(cT>g_j5004xgg-LL1)f@#uqbZ#D3^=J=o2U ze!azLh0)%$0xz*~_SU6$C@kUfzlT366Er*q(VXyEWp-d`tmg&u*96FhLhr(X%}T86 z{AAzSr<3DG#Hkzn;xx6|rFUf@{w24}h=@AjyHO#Bg^v-Q7cyKRhU5-7-BCoPAtibw z2&ETe+TcIk?nbP2$Dlsc`IX>u932e@Q#tn^WrJjiL}Y z-JLQnGP+mmvGx&1$!!48wJZDEsyjUcpHy?5i>j`|Q-Qt?FD4qKWg3*0VtUF~)B^k% zun?0r9kvjvBc}_-F&BFvF_U9>5iAJ4m~HU&{jBbfjj^l5yas)6!I&`NDWKqLu%Dllxca}3nrLWaXFlPM11BwD1Z-v zbq8JkF9L67!+@Cx#PI}&ei#ehm>O|HvM>?z0UJET7Tn%9pyRt0b03I`WQ)&5*H!WD z_lJ1^VG4p~Ey8BBKL{&J8`YXUE||`$G7B15qJvIOOoDYKAiPqHAS6qpFEIQ5Rc617 zeOb6~4*-dy$JjH60u6_68|6eCdJ{KF>-crPyYX8D9vOaLdiU@T&9>y}LS?$hOpYzO7kKR$li31p#><=_FX9Cp#EK^3w0< zInqPSObOs~YVn zR&Xy_!)#F$qDC1ks89~*>jP~~_?a*}7PqH9atzUSmjtCa`N6O+Q4yE8yS8u4zL?2M ze5G3R;OxWKhGA87e>$*4wKn4sofceM3Bme*)dR~D9q->x=HSG>rYkVA(EMf?Gu_jT zm&C;PXj+-U(D~m$lZ%OB_`X(tvtQi`9=THgez4GxCj|ErB(#4zj(jtuT?!IPLs+js z;1#u!JnmR9pc)S-!yVqzhh*bMKL`nJC#>_(=cl4cuJYB=fq$*2SOVtMA!W`@P6fg! zA3eofu0jSwUb0*Fi(jDqJAejy9^M+xb)9a$lbBRw%zcl#J8rQ+D(_*d zMMqU)#?i`5@8Z-|{g+q(y9XooCI9)_wPf)~vR(v$s65&c0ng>?>sNVtCTREXNiw~Z z7iQB1?#1Z`0OJ*wW&GRy3Q`qpdb)2NQ%#psV!zc()IhvG&)^w6d7MhcXI4|@0jfXP zVlKSu01%G|FA$XXFE>j7R850yfKVw%*Wrdt*LiMyhJJnn$x^dzIO2WpZlr#gyF(JV9MT z_!&nUR4#H%JPzXaRkzu3h zzCz_)bvZZ&iuuL=;PH;V@*+J&5t5`6B$sXXZOgc)PE> zNOwn`{OsJh0_#3M{Nl5Kd98u`mk89-$wO;+^8}e)_i zTBW`O*<&G#=lT$8Fkg!rc|dV(CbI>tci*mE;@6psQMLunr+7^M2|gwFjp*gyoMch> z(2D(+i$~t(Uy8CuMKhQm()NlneDo{OEBiX~i6P-EZ9;P0p84r5K~vxZyp8M9MQ$Ij z{{z^}6nm0*9KJfotHv`O4Yh^bX+g;BiMveQ0$zF}Q^udRch%ppDStz7%7KT=A%7ub zg+3fnN9D$JD{UN?t;Gox2W;T(k)5U*#vH6Dy!aFN0P8%m zanp=WM6vnxk07gRZtJ)2AJ%BY<-vk<9N(U}Pxp$Ep`xEAD4B3t`&(>9&;qF-2^DhH zS6^ojdJN^@Q}GXE2*R0H8-`9s*hCRdE532vPTK4 z)Jq)T7j0U6CJj*o@w2Q4)ddilVZqZ+IkK=vz&d<=L?M+$LS?IVjVs!eNW2h78g*-e}Uzk>m7_Q0$CdzcA0R#gJUvK=Qeqieu4ix! z3XJqay`Q~Rr3_9A>8=Keprgx4kRmwNuqZWKz#?XvqNKd8%2;i&PpVw)xKm%f*}Bq^sA`9DLT3n5~NMK;1Lfj}vkF z8wwWhg*w+||M@A>mk8d_f}v*2h_jI*<1udMiEnSfdU5}sgl73a3C$7^5*=}gxlEbt zs=t7Uw-J|7%q=H67-u{N*VDh>0mIM;QHU?#)I?E*I37WsU-yh~^E7C!yaf+Ak z^)E;ev%EIo%0Zd1{_uJ!|A-A(+#%^u0<2*y;REDCeuDy9V(U@JBVhE3l0O5Hi88V# z#h?EDnvV+d$e!k;9g!CLc)uL$yRx2`-r~q4jcN)@RC;%!8lUh&tgqlU;_kyQUdT(6 zgY@3{z}Yfu#c5{)@P6dkxT~x7PXVR7?=t}&`!`%fU&+8<2JBcC7!}yECg{1_;9u{v zl9n)`QWYZ|4VvzEY(TT7(wgjKAA5cDV;89wzN!i{$Gsz)`#ZZ?;JIal6+0#!;-41i zw?n3SI6h?#;7~EliU#P9!VGOK$XU@lUgStZFQAT*YHlF$p{66t)Q*_7>N1_DSo8;BTv2J~6^wO9eWQ<$#?*YAP@g2 z4{nVcb2rru*P-hBEaOCMuW=AcpKz^Cxa`td9&p)(mk{KnGNHoB5J!;kkn8Z5DC?2? zrp}9%(Qs?2I}MHpmT_7T2lu}!n^Xq-|)&dZaK*QNBRdvERYQVz?PS3Kk zyRVyp&SPB~OlJm&@)9Q56eNLl0SQtrC@J?rUBq=jIxha-Uoi>_oAa+u4X_C%QIy@^ z3>E+FyAioQ1882m>#NJ`SM5l-iI)M}S7y`GWxgBBGJ&d666%-+`+teJ9i2}?`uYbh9rWEc}!NX=Y+_C={tc^U3pa95?)p#0;DjER+x$u%sw1A0F;+H zP_1)tajAxp_GTa-N_lN2H_E%&DWM}FjaVvQqa9YhtN7Ih;(pf2bct~CXSnk;*;$H0 z1Q)t16Zb)wlbK6aV*d>>-k%@wZ4WVYP$IR3j{*od%>PHFlf&!pQH3bT(GdqeEf3$z z0LKDcv7BJPatsYwS(3+sWv|x(+rdIcc#oSLiJpUZGVzAiz3~{udUm|TZ7F*M5qE|C z7-xkj%GMu%VIF2nQ@3@|rMyGJ2&mg1%>G?Z3I<_y%e1Fp5A$Kh256;8+y|7xB} z=ko`dxC_(u#zDghx)Kko)du8e7c~4E$;~fi&^~MkRsVZ zN9WT05*tHOeRAKzYv{SC$h#-(DV5Vgda!U=@nld~Hd;A4A8=>U#xWCkx8^cd`|C-U z?}EQ23u0XgtoO%J)K(=j4_f169qK%a$89+%UYIagscs_7R*0?V0G%;@&NZNN2S>nC zKmO6UO4qi)t3#sr>U;af>39tE%M^QL=Y&HjINj}F%`9j-L z(btE6L{<6fa4f><;?Q!kPR_RodxXe%qC!;rU59Y64hRgg3E6bETZC10l-u}lFn5kd zBZz6`Ln>M=ukNppI+7_=j<*$xeqG9gF`-82FPp-SeyV3AqK21$VL^5@hG=|}5?MUl z$obs8ZftE51dC^U=N7R@=#8|b=WnZV0gO1!|79*gx9yz6Y&ISW_1Q%9Z0IMnaZ0c( zO3p)lf&DWP*ZxLRTZuE;77YV;KP$$*2hEf6{(wJNgtsY+tpQ~{baY-CoRykBRw4z^ z7#F)6N-k29cSP?i`-C2^W#c}HJikz4Jt8@@xe2NjN`N$M&^(%Dz%=4i5`816cc5ge zhsZ4b7!ljJl0@yUa(tUX|9V}EO#*dzDnOMrVG}hkXjjvI&wu(lu&{fVL(cl#7*Su0 zxlSiYfh8D-jLANQS`n&D3LNwy1d(r)yRQh^JpKw|o6=!(UnmZT`Q&M;lGqI0^@{{v zLDhmg?t%}0G9cPmYln-C_8n)-e?AW}u0Q{yeG`$r%Q?IBRQ~0exFHq@ayOtb3cpDF za)=ozBbiCC=I@U<)T$EmxsEp5cjyqK=+HGHTk6Do*7#wg3@d=rpE53fv>|k1z1DZh zrB8t6M?=OxN>CFRLDM$Rdwd8tIor(H`x$nDt>H8 zHKRtnj~zw}^W=A-FmI|TwLdu$JKtX%4UJx0%M;a&^j<5z3?xG!e$T$>;hS!6XLp(J z3dcLziT6XGi>mnJZqC6fCk&x;8SG-eNObt6_^V8Xx)hA44$E@^I_LiJh9?T}3&oBH zEeI5L|N> zIzxA}5u8j?0X5l+6|#H_NaR1>MltzUO5<#Rc?=Y8Z*7C&UcsHgvY;MoRv<}INGJq56|)l!m|315jOR> zZ*B5uO#M^9;k<#Jd|Tp$1;m_jEx*m;_hA2*DD15KwhXC$^Fn#Pvos+Xv8%oEckI9z z`0eG+9^8pN9=(l+N3~;L-#tG)xzDYe`hu4Sw<6iP;>gXl#M)q}#;geD9HZgv<{{wHiI#;Ud>l_2GjqT=(u;KKT%=E`svTL4RotB_yX@zuU zWg$Q&MQ`f0im*ynhoLq2`*UbFAzzNCOEJ8}$ zaD77)=nPQT3XxLLYf1bV*Xl?*lzAd?vWOY!04Tv{CWW;*ZI8)XU3R?ju>n8QT`#)neq2ub{pjPeUsxsipH+I zO{rvfcRFAqYGf%$P@}}gh-&VAcInr88L6r76lU+j#-mN+SM_*dyCBi-aQ3ER*fZtSlc zs6r!l(a}L!2@K^&*1DLl(43HYT5m(*v;N?`_?j&U9Z92Pc`pP(g#Se% zC^4G3j_et1qFs-W-9|E z+Ge+fu&s(#PVV|`tWcdca=5%!!OK3iR_$5=(}52VO;iPU9O>0|(3^JcroeZDivH%V zFS?L;(A)cGe$tWi{Xh^3=GJ<5o#1fvWR42EjNe+ECs-}FYrp; zd4T=cJh*OxHqv7}%A}uQLxF!b*2dw2n0VYDO?+gKnW0_HXIo7U$;Zbvjglg;d=1@v zW|R6pSX9bYDxU={gq_YxTEQpR{UdZl*IS+!7aLh*zC;6Ho7H(?-lOg-6cCRvUzI$t zgp4rH2-h^cKxc>PPC&gExo;k+2#CRSiG;pfX)XOVoe<_zgm;ayQ7MXpWyLOOa{R#G zrw(`H_KrT)F8D99<>Xe}GE5bI+SQ#{^}Wzo(&SK6GSMpwar(4-+h%ekm^4beR@$~h z#Jkrd8Roq*_`q1K$FawY|$I z-pkm$r(z(BD`&U8i!HVLwCdK{MbxQ88&lA;FDrR?xR}>1FX@Fhc(Y`3sOZNrndLl$ z*IVem^K?Um8my<(~$YbWey zH0)moU+TFY2(_%WJ83}X-hfCG9K`SelpqFo_SYeSy~pT9!*skDp#`Ve2OsGS`SQhX z5$DN1f8>OZo+Y;f&48QdRWY$)*WG4LoH<2;;>H*E-rlWn)XKTXvZ9coUwQQ_BQ{kS z3VWpWP|V4Xns2Tig7}PDF>es|DH`0fT8bnxd+IuF=zPYsv4G{*&bzt-$N9TMESjK%flib8F+MjSkz&Axrs1Yfv)eFD4#Gezsej4RK@Teo9Xu#fpiV5} zuaN9zYV+>6j6eIwWT`y2OOPJR=1~LjN!?lzu&g`;E^^)E%y*z;@`@&TfpW)}?0n7SfU+-NDXhA=$Sf}98W3N4uP|j5u zRi~Rlkh-a=F19(- zL!pZ85QmwcGM_dy+=k|Le_AZQC!pTj=3z`b6*;tT0eaCml@-lP2yeTi3U zc~7Ul(D?hy7a_Y>JjEQK=13!#P~Hk=P~Un&|GgUoD=yQ2tzcB0ln=SU0^`snMD17;AW@Z+ON5KyN8lbR$4jkaAdo2C38!^#$osSp-}8YmPhLTl)9_vzq{4dG~U-KGBT7_Lw&m^d@jauM1>OEoLvHDzmDac~<1G@J7!OImQu7&p<98j{^ z%`p+-{k>ddqauZMg$uAWaV}y~@sepAu;zL6Qq2nJFn$ETps^AKnrft%9$XaAvtkGsXSxrO3{N2h%U4<4N)^EP`6%&a**W)nOIq zyfYNW*u3SN((>YXovxstG6~iqT|yssu_bCdXk_lmbgd&^BW00zZeTHD*IxUX{<0u4 zb^j);#~Xd>W^e+-l=Nm(*9T1FNwdwlE)Ys4oop`Vv{0#CuGHl>%V)>_I!NphbQbRa zuM@uLUHaTi@|cms4VPMS!Bb+is7J_8jzV8;7O#iiAw~%~l{*OP5aVxE!H9!D(Rwz% zw0G@@6XoV-#6C**^s4wK318$^=e(%9GEM}AF9=pXo)%gj;_CbUdELQbNO)wTbm}B& z12Ya7!cf_p8La58Ou?M;otG3pmZQS#*iY=*+jxkj@JL^xNp^in2FtHe8p1j0A05lC%b?Q2r4v8U@t2sncX<#jeORx`oW%v6rZZyXN zlIOYhNtkUmk3YnDL7rbAV zx&HgJGwX`boke=@qp}8F(DCJnS{^DIA-rO-A=vu;hkuN@{N=i z*JGRF$WqvE3iw$xg{SNrR6FLFB-w8do0mKjJh+==zcp;pZ{fM!?wM#VHLsH`i~ULt z{jISuwESA%ss7}!k{Bm~r&8C}UVl{QU?+BX(hVbmcWCwT_($>?dBj)M62&h;sTqZg zRpp75q`xv-5dXP8)shtu#im9l>b8+FMDm%7SYMdn%7TF%0}k;PZN~kof@W<&*|Op8 zE6KL#Ui}X@&+Tqk|K>gh{%z}N*SdABXDMhv3n&UQDTP=8Gel4#^suxGnXpt&6xRVN z$3b{)`)8DkNAqlEmLe+w--6oceaQ*@$iG&o3xu*)~mlcOW7XPplylz$`-{BTI^ zITaC3@HpU{vF07*`-kaGel@5Ie47+^)<& zGO1dv$bS=S@Ap>p-cAO3pb8W{SO=C#Yks^BljMNJy1UN9Z^jkM#}D^Q?GS3)o@Ll6 zZ33T=`_=nPVvisrleQGdiXiPA+Orr;&Zm)bypO0M-)+fi?ol79ba(z*FU$=sMf2D* ze$&md?{gF8-oqdZxLc&ouLj{L_#D5`Qt)MusPg3|$ZdX|V676<$o=Mcs+20r4V73T zh2R_$zsnousL(IMaHzd#ct`}J5_fe%5)LC#1mWd>YLSEW=yhN|w|&xM7}z4GZdh!+%U9KnEcCv~au_<-~;V7Ppg=*{O?1yl$> zRk5hs*I5$Bla-7C>)#i?7PhJzDtL1RZ5=hpG4vDUd1c3MXnANArPp)meUFd7ZvrW}A~U@)xa5B*dSA z6ad-wA}Oy(xy@WCC#WE+a|k zRj{p`wE`mP&zR+w2(9)TsB2|IYdRCPW>C?d!}|Kuct{&LODJ)HA2EmJufE3w zRlIV{xH78Uact%L_2chqsD@holHbS-n*6&PKh#`sO~_UK7Wm9SPqK@D&1eF^v-wHD z7YdidOPNVd;so<8iJgl~Dv{K%S)vvjaY;mQ@qTz1p7-1PFnT*@fA69VfWLTe5zi&<^BFHpvO z`nCr#!NkO#YB98?=sQV)$6Em-?hAEKLbba2N{y85n4kDouSvn$ z@R2ezNefPVa0e^B(v!t&i?_3Gs|4po4|xd8SEaZln-F7tBXPWW;}nH0M?s5}>sl&8 zMmH+PZs1Fk8QIF`k39Y;h8l#>lZRI~`rtGc)AGPjv&*6;@k-iw+G#*P9(?C?#%8_l zMda6Kbn$p64|#yw5i`aCXct@R7)vr@jYQVew%o-vMHLE7r4t2h^vy_T<4|qj}*-G zPx2J`$&dFmis1THc)YKUa72%qSq-DM7x(-RMg&S(Hc|88rRWWNt+1~)>WDrv?ETcJuBj_K=WpXj>0c!WvQ@T_FYRBbZ- zL&C$?wU@@uvwc=DHZ*cV#Bd?=28E-bd7}}x`5Z){p`mhnn0oGVZ|Fa9*dI=LYWC)r zuGm#;Rp(xGStPmnLvVjojr@>Gx)56C>_@Sc%A4f=r9-0{OrZVvZmZ zFppoR$e==BZLBka8n)G1Mb}R?7(LrH^FRLlM_dMsHL9W7xad+O?)y+J ztJo==S{Gk5TX{Z(*`m>TQ;OrzEPqa}UU+wE-@kvAd)v~wvdAr&$uwwHy0|+qZC!es zOvGU(-0PJHaGGX8j#g5h?tjZJTzZNuuc@f>M2+llu_XDt>y)JH)|4DtC9(Lz9t>w| zZ1BJ|Zp$;RB}6>X*(<(}fP!vsYOwN>lH&1kCXHegis2E*s+(@DGB?zdw5ZI|^m<-! z4oCkg$wB_0&`;A%aZ#9P`{tfZFYcgD_BhoWi;SIElb4ZE2BHuz;zceA^xC`O^?za) zgzE50DB?_ct<%djh;1%L?(V9tRh35Oroi)IWQw3FgRFX8++>q*bTwp15|1ZbSuXn; zf!T1&g)65&Pz}J%y-tFKKL_;1RSz3WNz%(_h@x-=p8GiNW`B`QKY&z#0LMGs&Vs&s z{o%T_e7rK3aFG&*{2_;CTJRIF>m$jn23qqPkj4~X{PBcks~fCZI0t}*Tw|)mOp*py z=QsOoG>-=#OW1X|6`mXBz{Kv?$qc-B^KXK`aq=z_2zhuEnoCx!{XISw2+*gynJM8k zVYo|Rxcek);_htP@@s27WA{f??j}xUt+(Y0T@%rojhAFf)HvcNZZAYZZ`u7(`@820 z!`)8Aze&qte}mpkc3sbK=qpYTvt z59XjU#@;KHb7YyR>*Zu7`kcxy^)cxRoBZEAfQl7!o~qw}r zO*Aq&&UL<0*pyZieQi9SY07#UtNMC7ncYMKX+oEUIU2sJ5O2u*$aQ}hCrN8936^k~Vk&6u`)uA3yjvoj z)@3GDMrz~q_k4%+mBfe7ikjCu;4>=hi1aiW!ANWA77ejuvv*m_$;CwoQWr^&kWH=Vzk=kYDS zejZ88KijPLL)%HGE%yBxYixYzjo7k&Ba&FJ-A}P~`1_4)Qt(B-fBoh%=;!{rw%1!N z+TC=(dcd^fjfy_?Xx&r1tKt!ZTc<@_W|EmZZMFyTcSG%z z%)(=pesM`1d!<4C7v0{DK$5hWp~Syg>k8Z8vvB?`Ep}ZQ_a*IhV_$};B+4Yp)0&es z|9w2h!9K4x7t<`8jGy({J7(3z*zE`Kn+ijsm?|8}PPg~cbGF5;2lK$?y_IpP-p=jLHA?kl~FGVW!! z*$4b9Cx{&j+x4${2|d5!d3{fVH|a~C1;elD@2mgsfk?sb8)<>O#5iMR7Rr(|_~nL|Edt8lfr5qR)bPZFLRa2@-jIyJxA`I_4G+h7W}8+%dloB3 z$`NbC04rCP1ywF_ZPH}3bgQ`@w}kKuIfrTy)89Bd1Hwxl-6|z`XV|c$c;w>`a*phk zkR%19qPwpst5$m*PCy?b8mL!|+|X_Y?VK(>tNubo17sFoHC|-2v;`#Kxe)U*(9|=q zDjCJ4g3%)$U|+-}=K}oJ1X02a9($sY_!v0+=$ZzK&)wC7(NQjAqMUch~|P^)YU3OC_9-AsyrgZ zoULdzz2TVIqe(nOunDM3pL-CG_+JXR#mq0+E*$rtH4N89(v35?fg&8tiFcFkfB%7p zq+4qB1$BJ>Ky6I!mq(UeS`^A($$Om*4OT+_xr-Nld?K|%;#REECQTQl?1i`=RArh= zfFqq23^Mar+)Yb}vw3Q3`v{iPKEp5uD#z4^?e8LDVjehngABjmV&Yqw6hGzpFj`0x z{U{(o9NiQbxp2Jre4ClzL1KvdwGF0j)yILSfvl_^dX=r_DG9)A&s9&q_oAr^c9im~ z>h=5QUfm>+RxcjNKvv!ca(Spe>64-qBcBkGG?mwd2Z7}>( zI~`*Qvgk&P*UC>B#TU+7BF^M1DkBn0ON@M}Il%BBYsUi7A-?UF+X+|-{l5?3YRPE9mbTW+PNA&EjuyS z*!aL@u%`}WMVz?J8IMy6={f=sEcHc=ls~bqk1HYAp_r^vn7jLllRHw^&Gm5qh}&^t z@K!jRTA~AWH!@+mvZ?|~3qN%rm~_^Q+E2|e?IGQ#jngNyZ*!?9`&f5`=cY8@pR*s=Ygyb6oS7kTSa;5U)v6DdQ5?e%4@>(Ci}p%v#f|42x#z3kuf#PkYQ@rTb- z!&camI6Y-o=9xmP~^ zK_an%&&g=#1wzx9E}RdS?j?gN_Dh(*n{@Y@+4A3;138Z65P}E|JN)@0+vS`H*7cr7 zU*7+uDS2c*S|NH*ZM+}#DJ{NqNQ^TqGaERcd9U=N$$$boI_QP8ft-jd>x~+5QMvyf zAid16lYIP-OU?c5YkKH7yj%fQRO`;l&uzax8C;Voa~dvpcpxt%v0*a=Rw>%bF=MWw zTe9ks)L+`As~9jIjI^&3MT@ypB#67-Iy&074h#$1hS@%?x)}P5fTNRiJl21(8RTJK zgfGJQ)a3IE7C5*JZyl$kFFY4$T>MD*7}x9>nu#=@koJO#HnZ>}vW--u+pbfy>60KJ zy9N`4O&}gO0ZQ*>^puxvLOfEzS*8KVkftDp(AZ2R=#bdaqibDt31nus&(dFYQP+1l za-9Xzy1-s$x+zpKi2{k?(oW>k3L&ISaxRi3LP-mBrZv0P9DL$L_Tc^Pa zZ=i^KX0{>=X9MBIf%V_yY0A0!(5T(@>3J_0$V|_T=GOa|E2USW?$2R8RSIWdN1BR# zMGyRJ^3o<*GK_SWwH_VrZwBE8FBQa*@ixhL6$ihR+|EO;5Y8AT#S=N=rE4%w^YXzW=cn*h-U{w(lt-+X9lsDx&2X}+g>=IzV~ z9(@r0_xR%W<#pKv+1Cg6>ew;4YUxh)58B?~o}q36;~8d|kPj*YMaFmarEQKqOy?_& z-pf63!J=RNHN9&)Zn`G6?9yLYwRLY7bGZEhlqmLISVZ!vd9bR+B-fj)d4{Ru>0gtzYR<;`f4*{m|rlBT=F9lDRrHf-l=}K zcMfYbKIbSA(7Ky2Mfz#}{{cwq`!*S=ROfHv2c*~#La5NWj!tT(YjKmF{J|4>rb&>V z(-VC(V}E&q_}ciccbO{<_zn@!)xLt7(8hyoI8%B#qVpw>wqSX_$G5tfqc2J(!GyEUkJwaieVCN=>X-a;RTI+$Pzf zd|kCKMVyj!cq#?!gt({i{gp&TMa9*!==7&ah@@Uc9{LO7JcgT3En-E zre}0LUsFN5xar#D*Ae-Px9tC?1t7c1{|GWFJJ~p)sR;Q$Y04AU9l ztcOwt1=puq60#Crga?>*apo@*Y|xJ6nL-Vsy$WZD4^B-W6xu$vWf%6M#zP}Jhyid` zi*=KBV{Ik~M#bkO^aS8_<5^t7uDtVPJF(JD(ZH;vy5ioI*kt|i7!?9khSOrdz)pU> zcs9w+soD1TW{_S#;A!rQWKtLWs_XH| zfrp#I{@iCBtZ?mWuTNaCS&o zJICQNHfADDl4sN)FW@;JgsPI%99~<=^W6@yo=f&~`Sv|t!0}^f;H>VFgQxoQ{n)g- z%-MHLK0{@w_OZqOFGj{gh{d+-=8Y{bH*cCpoSNfUwB!C#LNjmoaws&jy1Y$t9BU3~ zxZ*1!n|m22*v^mPmiXg^F_Q%11mC9(a+3~udu0-23+%#V@h>kVIo`9>KbpaO)en2) z=0icGk7Nnby~t+clP(r`tx1e}zTTJwP%B29Gx4MxYRkl=I1Ee$B1yF=vdvgfzGn%z zh-oFc6mPuL4*GCCkW9blq5%*P6yX~x><313(3)$EzM8W%v%7MLjfvHXm3Io6j#A1h< znBIaW^t$()8D|6m%SYbMo}kCSvnCC^F0D%*^;$^r!?)yn?ZONdsP78WG%|c`1uLUw zUexY#yCQ1qey`)IhR^|x8{6Pa{Nt_W$1o#tVG2gvnKNySXmy7dTkqUqPdsR{2@83g z|Au>{ho47(hA9yaoB+d0VgSI_igea?i1^Ef_y}5T`R+a?fa` zpEI}NkXStYlae2&+xTXWSx@Pi!qALp5@WR41Mi{!AoY($^RERY zU~v^jB9=#6|Fu9Z_ke``-dl99x*pYxt;eGp9Hqd+M@xgHj@lQV+!b33WGgI|xb`Dk zo%e*gziE#-!{Wj+``U|p!*1_iS`#lba0Q;Z9OAmABctqnVE9zgqD#r=P0#)JA$&V2 zEFUS(hh=>znz8sbV66mv`VD{oDo^9}0XQ9Ro;b&oalJ93K$mKht#Uo|wI(&Az@H|8 zDL0nzpYcN9d?NB;(%Pgp9VZ5x z_z32(X6Zj1_o3Y$G~2SF_A3=_`3h^w+Rk3}ZC8`Zk;R&5iPLJvElMjMx*7gOX>-0O zI)uYX($A5y$+NaaWOKILYM!^+#p-!ULI6y}-glnsRP4WGB3WT{Dy!>+Zz>GaG`!8N zexn%4mYzjaTVd^X(^CE6t%Nd1mhTTfUvtqmuFZ=5{2=z@Z0JgF{ezlbb&C5Qv1t}) z!awJWG9%_JQGF6BEKZDS;T3W2f!~f_a-GPNUOp-jo8)1d;e}nY+j!Z^NQmu|Mk_w3 z&v}1`_+eVS-CYhgW{a|=z4?>$soYY(pK!0mLk}<+Lx5~PKUyRaJmJVGD+)sgh8KOa z*-$zhJUpsnHk=pM1&PGfPn0evc#J>1_hV?!!NPp8){W@B3!lI3H!l5Rugd!#tKYk= z1RO$z6|<`LZjzA?)pBD_%EO?)paZWE>oE+3ABqyceqh-@*ZDruIk1)Y^@E1rzz*Mh z5IFO`Cs}`OZbmDiY<*(-mit%3XWwm_;2akfC%)XBt}B+l zkMu@rJ^oY`B=GEt4vSRou@*bvOfqlR0SV9rcfNT7KjtKJGJbqe6fc-;s0dCmH4_Qs z@a!|6qx5By3nlFBkjSIEQSh!!df74ukIC!7g|&9!;Ef}~-ERJs_+t=(VIN<_t!rWD z#?>#g6CxzSZJzT5x|G#9xGOeSeu=4GCO;kjI#IdeWI5FS4aV3yjGxVKB&8Sv9r)Ch zA_R!?Z9woRL%Hy2_vl4SF;K88(!MVUy`?duloS6z(r%Y0#C{euCO#+Of5XQ=UKhIg z@ruKaP42uQFBoxNctYO&zm5eVsgHV~>9h>?=rFg=nXRhL__HU<_jigyCgvX|88Ayx zhbG_%fH;P<+)O_ApNd|D3|ZU3n=z+1QTuE+_0Wl@?PZUF3v%;o3qJ*=M3Ucgua4$$ zD7L)AR9-@;{)J9>fh&hdo;@$KfuP|sd!JdDdlV>f1al3I-Ke?(U3i^ zcnZ-_{rP$;NNn4fxa*JL{Na+NND1?6-B=ZaIHpRh+t!>F!O9YD<*u|Mop;1k=CS_$ zetyJo+`NU(X;I?lg9)oTFX7P<74#L)^<&1ws2rMO8$h1o^xD-wHMZY1Kv|>>kVE8J zN{bLP$yKcDU4cPCj1B8{bHO?8S^nCPQU2?{xYoM_ zpU8IFb${VdO)k%3yBJ+j;Wcq@sV1I09u$Bm>x~bZ_HenoH=V{Ux_A=I%?4K)@A-5sxe6@Ujf2c9CUd5*#^G;M>aw;I2 z0WcHWrhj_0$II172^Pc(Tbm}ajaO>t$@soJ=5ZXu&NicK3+S@8fBr6sF3up1qp%vS zx*f;%bbFFIJ7W>UKy&v<5Lp~6*eL$@K`AoKd4Q~B@Sf<3S@wQ@;Iq$vZ<=o}3x+vkf4#Ha-wTh}eaO5|$ugzx zDcj2dB@$isW+a{Z=jHo^6^U(>s)^g|@99vdS_8{`1gYQnsiKV5TI8v=I>gsref{r! zh==F&p{)*0RPc2bWzp{v%=M7vU}KP0G3aWXL}`+42AbAe)Z4@WTgtk`otwK zIdS6uPUv^IsrB8U;y`^ldP&acE$QO=Y0U+i^8}{USXqP6IU2|_pACVg`z3uRRfs}$ zIPnwR(((2gc)e2EY-w9xVwJFc$@x@w@Wef1OVp@T}l&V-AfxN~Vj4QQzyw#2cp@V`}_)mS>G( z0es)k$3F58iKc@Uc6i4GqOUY1KyoDh#4O(!LDQ3zCJr)`>*82i5{C7>q5B3#V}Rk% zdow^|6=m+lWy@-KZ2^-Zv2OHDc+@FAF8|w2nC&%bnf@n^pTb%c9V6KuZpr%A!X9Q; zT=`?G=mw^cxu-}#@VH7+CIB?31FzQTPu_*2j8-AGv7ZA$Za(9?%)ZJjhC=A)XQYUh zcI)m)X)cp}Phbots$71*gJu8tIBYWkL22IV+SQ*T&ws7)jBj_ zSb?|rCP{J{Zml$SyDWlL6As0@0aN`qZpP`7b)!dhKsHe&^5FmT`LGVY(P33q6g}DF z69YJtT}DAASdIQ7Yq%4^DnUM8|ARWKn-MCG$K<-nA<&E&*GQ~}-Mj$2crY4E}6i@3rgEp9Y`<^otwEj@(%kLgPUQIYK0OljX zeca1cf{jv+K=#D~Y_21hMo2=>Nubs@k)w;R`V`$W7Q=UQK%tT%GY{UmJcAd+pCeh~ zS?-hri6TXWu(f{akw0}R@RhL~w^k2!3v9>bGVkWREqpZ4-BD+dv{I2IWu17iJDv0^ zPlE}u(gIU4t@FJ4Tyesd5#o<&z~ST5*@h&8BhT@WvJoVy{sogu&@@p5BKM@fkfr!` zErQRc0#sQ5?bSsBZ;a=9m`V|>*# zQA}sQMY!NepMWCAt?MJi&94yWD8-vMBS0-?ZvOe{^9n-%7tk#&3!NYGF*x2fCOGka zbL!7+(~k)6vg77N=*B7?FFl4af}O~|Ay&#WZa$IX%W7LrkMZkoYPhp^SAU7X)@p=Qn^w~E7g&^917HML>vMo?Rp28F$Wk?0&;PnXM|D}DN<(hWs5W5U$;$HxM$vzjnJHO*vj%&PV6f@PG^nl; zDx<%>jj#*7-iQ=iS&4%J_lGB?b`jy>4CCN5b)G|ZFNn8tg3A22*I}xnU4i}wjjX%e zIxAosp6Ts93z%&IsLg!HaqC-2QcXZ6nCMK7X28g1wj*s;RM~r3VpqB4u%&pU+jH#5 zn^uA&T`lQ+Jkw~gf1zn3*{tLCklnmxv0{_&iM4_Y_0CNzVf2&kACta@iC=WiTm##o zMnIJvsXLZJ2dtyZ&yKWp1_d?F=_hg4O8%Okqq^T!l3yQ#s65Q?enlfsAm+~#FvCHH ze4XWX*C(TE6rX=DUfR+dFkAMmoLy51s6JecbJ$lCzI3bPMdL5|bdR$SL~;X(X5^fMq!TS-B95X7$vJU8Om(aV3NNHpqse=&M?@lF6(~0eQ7p} za19x$JR2O=Gu@3<>Bh4`L^#*-Ryf+!Y^UhLx<>K;;J=oY6S3=~0R>*QxR>6EkMds9 zyN4^am%$V8>JUK%L)-|kh>!lFf?S~79xm>>HnZ=&wpxGnfn=ST%FK!F+3w2n)~ZZd zJe@C(qnNXz2|i2hNGEEhJnhe_2rV9bM<{hgLS?!+ksC;zzx^+qC^(ahgodY$CT6dIp+ zkPkX{xrwvY=#uj!gzfEJYPVv9Acg~gY? z>?`1B<4k6dmn<~F1kOUAR?b?3EmYF>9e>B{v(h7f0mV?=8e;3nQ{0zZL;(o>{-bfQiYSEFPY zA9iQN>X9c*FQ4=^sm13?_M!774v}T`B>^VRf~ua}Gdc`wn^+6=uGJzOysu3D+#h0C zsR?e{8TY0%u%}qB43kz=}B+syLRWT zt7iM@Qmh!7Yj*UKrLh)ULTxTTCnt9Vn0-A{WXFAL!FF+^m}GeE1ihPrq$dA5n9=5r z&-Ik9-BjzP)~p6Raq2cbuSViBKCNH=SPQZ7Mx39^f$N=v{Y;K#)p!m;0#_@7L$tm3 z>r?HThl4)|JKLhfijU41pG3WxGJZsP4cGUTBKUoD7`ZlInLEe%cB=11?|!;!g1`+s zG+APOWFz^XLtX`~NGYEZCmg658;1##OsMcyKOfW>Xu?&1iC1KZIKDUQFL5P+@#*|l zcx|X(h5`gw<-*L6_#7^Z9B;23!C_=ePIZkF8btR)V^wKrsq=hrS`nIjqTt-Lf_W!P zCYk_Uj_YswEqD3d*cAO$k6vpN$`#USuf+au=LzaLqiktMZNmrmzOjCJ@r9U4jLf*V zw%6*{p{#fA#BEJ! z`S0K0Q1$XixwdDoZW&>rH|m?uP*>g65W1DqNs~9qBQ$qIH??-UIsu`iFgEY zy~Zc>BY#>A50<(S0q~e*=!oumyl%kkvYrbHn@Qs{`!%(XxXQ4qVFN0FEvyK^E7e`x zu*kfnw6inaYO%f z&9EYx9*j6fDY42eXb_#0?SSS|8oK3`Im8XWyz4>K&CpX_=U~nvIh-2dCKqT9I z0mzFh4G3A9zU{{^&R8o85Ux_x>zpbU{&EkmnUjG^Q_y$fB zhk;XU=(u(N=*;Ew7PN^-n_oJc+(xkJSqStx;xN_L)tW!`*U^95ZYePH|FZ4f7?B zNvY@>-6dMK&l>GBAVxHg7H-ECUv!cTUaX@GgJht_MM9CPiqs3&ALUn*Ulk1cL3z!E z8g5B|^SUmq)^B>QT?%vaE6qz7;dXzaB3y>uhVF_6KYQr^Csq0AG}*0r`}43L~w1f%<-)b<7B%v;qDCmsoI_< zCVJ|1Ru-1s;adg+A>WHG{;5`)2t=rQAz3F9V;o7EK1}0|o0uvRj^lZ^HDjvoZ)2cx zX!$uU$!p}D{sS%LXzRJ1ZsA>i_uF8C?8x>j*{v zq+av*B8~e;$JL1xxv)+b#{iV~(+_n4HhXrmU1@A=jG0LE@|1gtpTpDM=oDtk6&NeA z9OyJi;%gWuSLVnN`g}%6#bwuwwu63nYdNFq1Ssq(^gPdPm(V*IKuDfgUrc#_aL;db zOBgpm0sPg^47O;m_i-1#=vM0Xbq{i+e?{+oHMJQ|w9TtJOhq4aa21lZ86BTSTlVsK%j`Js(OOn$)L1nR}3K0+=&O2?m~< z1Yw}wDzw@A`OvDS61K{~X;ivUyR1NA?1EY!?;Xy&XL9&;k5efjD3Zcel3l5hVBa7I zK#w%LfG4|Uf$c{lSD3mn+SR^jto|=OmualeHXMoU@FF3@n*VjZT;(41Wh(}=n{R9f zUvHMvS4DD6esqai?)@UWHqpu?<~5WeRdwRC#kelpy-xwej>IezQyOI4gd)WMDz|K5JAcRo+98OiG2t|OW!lt*)kg1{rgmss{4g8J z-})wcOw)Yffo(-CI2pzZR}t+$s>1b3KUPFzZL*CQ@ns$Mo(47OH_KIr zcR>w{rPyI^!X>wO*hBN}?|B25j&enKE*~t4t!@=Q?7tb23&8z$dzP~`KX#6ZJQss6 z7ot2Bgf7rM{<6MY2sp*e%yKi(m?aa>@21q#O_Z{zn1k;ocpK7t?W91Bp#b8wlfdkl zu^D!1$UNPvA)17@jF7I$)6fJ!Y-L+%%2xafFB%<-kWtC@AN(I*UjY?WxA#p7NGc)% z5{iI;fOLnTptJ(g14wswBZ|^WgLLQ6-AZ>iNOw2Gz_;=7LGS(6d)IOyESNdx?Ah^; zU+n9(STNJqLKvlpAL%!nK1>S|I^7b?*Z8_d^P#I608J=?6ep$=-?ntrLvVo*0IO;T zs6ySz(Skzj7M!S^cKSjQQXjsH^1&QsIgncab8L%(tjN~C}_HTLN^ z7QhcCPgIOQg04GI+hjzjNsykZd;{~PFJ|qn+ODOSn{n>(6^)l8eChLp9{IKH)mG7M z;Pfsws=?dViM+I2*E4iiM^lwHL#j1>kKSXWDqws#7t5{v z@ouDIjuPYkQfCOsL|Fv*09VJLT+2j)rsHf4`(6nj4{G<0D&BD4zJm6AW|Du&L;Ics zYlS*-3J9M_WJ)mZ8gc3$nHcuwCu$VR34H* z9KZA1IK}4Ew(aV9L%Gw!4xRVo!ADp4rv})z#5%w#PgU8iT6N;bf5xX?*>xa3{r2;N zP&{la1h9J?!%URwAQ#ph3@opvpa5bB(XIYdB1F7xK@Ap%N6vOAIEKD($CGD}dEA8^ z!D)doZG;P#exS)z<&XrS&}pQ8ycErV*n7sO%gI4}&Lxm+8}E+Liz47NObe;zO51!j zh|2f4KUV0H1va&$nR9OA68Y@+Okx&8Ame^<+i$enS)*M*Riy4{4r341V-x$`<0AJu zx5C2+hP_p<7hX@qqd4vJ+#EF-_U&r{{Yg24@5Fi1HbQaeO6P8T#vxf!R?9h>!*ord z`FeI-Z6*G*+6KS=5iUrTT&N+zeO?~>5>oNoq`uKHYE_cUO82ax&mMGBZ z@kf4PV7(5J847(OXtH7<(;`Y}V6V?hbh7ikl*N^870Kf174G9rzUDJr(973J)fG{}Pv?Es6vK3foM|0+(&lWcl$p@lZ335( zbQq&Xq8AOLz<6UXj)=h5RaFg?ox`!YCKWqx!dmmJ#CHqi+*Z%cfDF*g!;$s(bdx=7 zB@bsI9a+c&nxQHsI{@Bc6|px8HviDJr7wSm?Ii_1`e@r(nrambr}E6NCuHV5Krrd$ zd3R#58H(gcHa#y*tHh5`SjMitS=2w+dXO>yXn@3#zu(p~vE6f}`0Mh62~~VsM0#wP z_2}mA8}vZs@yMXE=fhOEM_rMi@b~)&^+VZcT-DsCPRBOe^uF-7>gWuJ!h_ssGwJ66 zhG~cF9EKk}xD}gho=-ZZ>u~4+;epV+y*?lE_V&dNR8glVQ4AWTIak5Hbk}05h8Q2@ zG((=k-K{N#DPEG_z9))K^dg9^SP}pR_*{kRUwGl!uk2`n^4gDV((%!fj#YeTGS@qb zWLrK8i@;iHq%)0y!St0Lc1a>?inNS2K(1P%;KrADxr9&N2p zDDIP)kmf)`&9$ z8$>m(wu8O|%xW&T>L-_b61n?I$Cr)OnV244&R$nyBP$k`&4t_3-l@OG2^77B>&@^} z3%)lyrlCO(hI3UrfMVZav!t#>*kNc?*IRekzGBUn>?!J8yt8@CAI{@He-!%-OR206 zL79#PcRb>(BPPs3?5SnKZ^`h@YCUyXk#(J}vNv~S8%b+?SMq8TQgVi{ic7#d9W4M1 zFkKBf`akHOYqZ1oBUIiQn(CXr-wzs;_{`eA-&Cxc%hATZEFrH3>muP?qizaAB!x$^ z%(%HgZ@oinQ>n|?+jEDPj8yNnXGNaPhxMA}jvTCIqmmnwomCd7^WN`YymFCWzAQHk z(Iohw9$T`u*zn|#{>&5$$ViNlJQTr$9d{!G+(}8tH4(g z&9ez=s1;sjP?$fC8mlxkQy{RTsk7R6<_iuoa&R;#R-CAnOX+mR3u>rg!Fy zVP{1GIW2}hvoBxgdF{7MCZzSxTpAu=m`2d+rz|Y!oGKlXUIB4X;edL2*j(dT`m6@X z9JDzb5ilOLwA-8IrNMzg;9?1oU`^F<(JJgG)oOJETG**Tnbk>yvFPj`0ce7U=GC%O zjvpZ8e`0-L=c3J}Ow(Qn~s-1eJ`4|aWT(@BAyLMl|#hwx&N ziJnRSa@(J=Q7?NS?D)h~y|VRloy>}q$_(G~klC4ur7I?P;pHbBBJH!{((lx{@wA=; z#o&JedGyF;I`8h3vT1Ql>;3cnGdFbJB54N48tD(QZCO$(&Y4OFQ2K_ay=pPlG}Q;~uE` z)T2bR8ACz^J`=P7%+zt{SnSW#7KBvZ>D;4H6+kX`8p_Kg#B@T~IC?oAX)bN? zSKp8Xkc?j!wj*5;G8=;U(>RM>Y)BL_<8v5J#T1IomHp^o6^WNFd$oSKcqv#R+GM_M zXIvIJo?;uacVEmP?lM=I(mGW0?1R&@cT#pYbgTG`N#fANJb*w#)A;ogEqDbm6YjKG zd;RRQNGPD$G~f`iyX&n%s*44Gbnqj&W0Yn0Z6% z_QXGspZ7|z|AhR^VbaT73iSika-9H{B$+q#!LD+wy8arO5CNqD+Yt?>6ADvH^k=@Z zS)(U+XFqK8KO-O>j3#qEcn!2Pn!Lym)Jd6QH3vc7P_PAS{KQEh%42F_|3F!`tIg_cryc z6$iSXgmi96sccsqL-N=Yxja@kkgq{8GeLJTSK|Fv%=XdpPZVE4+-I?Cz6}(M#Aj%5CFbjg4Btn? zap15Ikb_WDYY}C|NLzqTc&qM{@Tk4JN(OLr(ShlwvnF_~XOs= z!ow^8oLonF8k%`-mSI@(UB6|+t$J#4Jd~W6#}$W~9D0p6BMya zY@Cw63^{9qxT;KOiO{&XOpHy9R5If?N0E}Sjs<*Qhzf}6q%Nb5|Ic1NK-d6)iy%YM zv-NMdf;N*0hr+9f>Q#0_5A6DbqPYv6Bk(ZaHaX$8usqsO?&&{PLTac0U5Tsix=_yT zr!kL9Y5;jo2j9Ne;uK^bnQt<)rd=>dSoyi%c^}o*)gIdR=Sh(?+;~k*%I)?%tr{Mv z1Y;v%O(f+o&C-BED(%v0w);Q;WOvnIFfW+5%^+dTRe&Kr%8J0gbUo2h&Vi`>bOH2; zRTxSobI3pfP}`*vW3Z%)UHf1x;KtNu1Zq$NKx}=Hh+uyID$4*&NDM9$`3D~*opk4( z1HByITE0Y%Gr(ty(i5<5#*%9~IvI{Jze@RR$BKIRC@~42>49eBs?A?Z*_@$|rn|I2m zo2kEgosI1hQWp>fQT(VIvbAAfB}o>q_l?7mYsr7$VFWvUQg?$U$X}lYovQ0*Xq1`7 zLb9;tVdVwdy32v2EVE&XIzBu&j^~z{%0+3DZm`Y0)y5AjW|LfPi3|P0Mj?5sgUxDo z7%)!0RLD)6*QUW&QG0&oNRW{nqeGTU3SSmu_L~>Lh)rkY+b^`|@91m*3`7e5MdxV3 zP!?K(BCmSAG_J+?KpU-KntQ$*^~`ZC?42;~FR>SB;|SzA)J0Gb>+WxEtq|L7i;3rL zo%S2~mP>=y)z#vMHXmK?knvc(q*GUElj}MD@niP5%f2cwJRs@@ow@rA8wg5fp15NF zif@d~69BfeJz#S}B30Z_4J5J;G-#pwSM626FbyrYX2O_`XP3zLWPa!VF!VKzE_zw> znay>=O5yUx%V;FcRMix(MM$&$iUV=~jq?LGtcIt7Vevim^XCip)4QETd%6*AHx{FC zW#G8e?i%@hdt2&NHrz{}dwyWxA2{#@xJd5^Cj{Ldw_a>tq@`8KiF8SVJS{OD=?-c6 zd(I#_gB7(=akA8CLEL#G_tocDwZJWq?5RZCmfb2^2@l2nLl1X)B3O_$OVw1l>wW^- zVAg99U~&`LxHi#X#Lk7)AD>Nt%T#lc?bI?&DjES3 zWHNg_&VGt&h_OyhvNicpi6!GVhj*`V5hm`Ryt~E$eIi978hdWo?KSp}CgsR+N5IhH z#dl1cTQMvIPn4d@wIH_m2yyBaYR|Jp_O&NZ9>1>ye43D|(1a`D5y*^jlGz5zDjZV)dejs;a1i@I95WYTztRD6LBnMcDT6rA5U#n%h_e%aiP;5^m-u1 z*{#ekMnJxXhTefXJJOwSH~4w6=(eMyI2aoNK?avvozE2aaUf(UA$D7nw3uON?zl}Y z(OX+%auD}TcH_H$9St5RF5qm~L7ZD=xyO&7PDOQhcR*XE0E**boT4;@?Eioq3$K}A zR%dUi%TGckiHsr|6Y|NSaRVWgAkOEm%ylsNFwW!XBHzKIzG^J`BM!M)jSb&Se4$3x z+nngU>d(IEwWrHnt9#nC+D#)JhZjhm-HyO%EKuP}1?oMybEjOb`=uMHv|fPkI_@>a z;mIe~pdrkcHQNa)kk+e|xYOiBqtl~!f`zvxBI2f;`Jn^zWW z8hqZjZ?!sy_=Wf$ z?Ab<=R*~P`zaEh!Kabs$o!GFLr>m+XtHd8&cq!}T;0B$z;hs~NcYd_x0Ay5m##1(G zEZoLbid+z}OVi5`;7eHNv6Px$E)uFA0c`zRGA+zx_uZ-G0Cnv0@D|Vk+bgc(vNwy$ z;@~bl+D%tk@|B?^@I{M3ieZ^*?HP#yRI61InioS7i1YlHNh&JF{lGi2=FHUTaL6lY zs-`ka0p;azAyeJ?8|x@O!>`}?+|D$5WTXK$dRArgc^>$(HMC>%kd>Y zNdAhd0a~n9l5I^d&3!DEDh9bec>O3z3SB^NyHfp2IlMXcLVeWE#*=k- zW7Yz-)RHSv@dDNDVKx8nC`<<#k+OAzt6HS}N5pPr@(|aW!cXgh`Qw=sn=ca8{+Stt zS_4j*kl8((Q-QAJv)fq2Bb9XDv2ui|;*QeeH~nK{+wg)GwTQ|@7MAEk7it(Z!YvOIqolpWDcZh&CAJ*w6bC}n>`w} z|IOEUi9_Hvpi%WLoIgWSqe&t(_)=0i{2$qRSppD?e0om zyv~NsOB#^w2j}1~{fjdT7P7(1zsdZ)1t%gA`D42Z)k`7~OG5(D#k3gnaLXP+rtG)5 zU!CR4!w=gfCR1+HJ!EYd}f@j7Q zG0d*XjRQ6{^EjuF4*}0MBtkm%B|lp2Y>k$Q$RYe*H}%Y>K`28R&lAi*gs2-WekIqDZ;}tFPxT&nyfPYE z&W2Ve9UFt}wRIg$YY6#D@Jc3toa)$abDqG8^$vgiI`uMRR@P3`Y1pjW7Zobwp;$%% zLL>a4rJi^uhP(>Vb<0;DFxOqPeQDuc8i3I8pqhG|5%^85tC45dB1`pFxLv~S0fi7I z6tm)_36I;WZkbj(eZoX5i#`m^TtIbr!PftqVo>&^I-1|ls)_+9M@rmqb#h1k&`E~=b8*v&Gn@`GidAiJE>1Br@?++?g zfGkw?;Zlr1#Y5>zp_Da5p26;~R1$BFGn6vQvZyd5udm!Eqd6ZQuy;{?oGMksK^A1` z(C3~x$Jm(t0JIq5q!f@7J-gy+)h~0;muF&Y!Ub;h)1b)Q8;uQ@9G@bOscjTV&fnv~ zcw{9{gQ~+X+lUQ-UFMSW z*AUsm2)U~F$XAaI>T7hL3=4c6JtOb6C=OoVrNI6pus8UI>_sm?+n!SsU)@|a%7-*Yl zdgVnwtSXnexuy~AS!(1MsDbhdPEhXNX;6nc&f6ziZrsP;^P{GYA=cMF%@tv24W}U z;#GO~pST9irdgi`G7ab5BAXfAwEl_Irt^ast2^R%Q51An-z~ZCHPOUn3Nl;i)W>bb zi7rbYrEe$-o1o2@w7ra9<>?T*QN3F*N5P8!7Qa5&{HyX?<2+>U#_S}g0OGw5GUsCV zcDiVS-57019ni0;#RpHNfplmOAG~<_$p&b)g#gQ|Hqk|X?S6Wr zJ)Wp`Ec!P+3D1)9x8EM8GL?Tx`IlG#g(n-*I%iIV51FIv%>}vyRshaD?(lfv-tEQx z?8C@zjfmbVVnKrSX`x?}F@TTA-T?BSSMZUbrg+5VUv#ENc==1^Jm7{^tzXyL^1%w>eunbU3tP$Cl~}UU^#e>$oWCh2{a9LK@NDOsr@ev z+pJD_({X1AsQ#{fl}OQU3A{~6n7%)wXbFer$cf!`+57whRifZ zFe-bOLFExf7Ae-89eyAJ`{?1YSIdk*IETaNX#-64`jUVJbo**wdn+{SDlFlu55-W zf<0Iw0#4X(abF7FUixtxSp7)(zJD>}JPTGp<o{9Qt^{EeNGJB9SRCq3k0vT7(6;I{fHYfDz$}pK2_pc^OK9~` zG^4fg@eVMoNEAjj%n@FZ1PI3Br<^^4(22v%0kL7%%NjNNDch^01evrpoeXkElj%K= z?Yh&RML#fHKKRYRIS0>At zgbe-j)yvb)a`?CL`E~Ue?OK==b6zUkSRYJo33ME~)R}D@q|dAFr{8`*RwM_lT+13H zt+Gp-W?dmu=QyfZeiMdPp;!Z_rOh-NGg)iyimBPNyabwX=|>Jbd4v4!7g0Prvp#ZR zimq#4>Mr&MiwebD)h=z%%hT^yqX`~GE)(0@ZA@mu-~jF!8+0Ka{v^uj=g()Z*wa0A zJ6-~4oCIm2$E#3M2boB&*JY{&R=R7zUn8xj;ihW>|JLQ41QTi}$>DCMLY3WGb&%lj zaydtJ9i)i51nhp90Ee|L|9zskp)atEwhn4OGk67xl8J9cn`cz7bXbf!rHkQ zT{CL4P@!WX%n+JERp!b!tDWCqcxW_)(h?ybi9f@Zf9zgU7}}tc_851^%zm1dGTuRW zZ>fWZX6Qy5{oCBzp%2EcxVBIdWkxb0J|#1g5$V#eh;#@DOfIjUKa&>^ef)r3{@dZN zB6t`8X7u6<0LB8e4i>GZUY7YNtCw{ru%1rro1lQsr!C{Jd!Nk5bE1H}`gCd4W}Qh$ zr6PnjAg@yVuFF{_&HX@A-LTUiJF{^~0DIO!MDVdW8jpqyV|p)mIXIA%mzrK-G!{U2 z21jh=BS}+G2+V6`U8-|{PBV9!@Hs_ANwy+1E>8~PxDr!-VxXEYgb0x6{kp$0q7i*R zLgOfW$Vn|m&laGJCqY_I?p8p^QZ!v}zLN z`lA<#8&?4M6v*m}gSIynQ0<0j+R;B7W*mZQXoTula*R`U^Kdv6Ql|r<2UGYWp$y?u zadH8tLF)jnoy}0@%W@W3dZvdmwG(Yu#rUofjgFMIkO2kbH2+=tp$@cPv{>D#o^?JF z_0;=3It;_hPq%{Hf%Jdo+~7p2J~bab03L-=V}ITBwVeia$QlUB75OJS&;0%X65Wyj zBvMjA%Wm$DaY(TKZEv+l_h7%$F}e-uKxDM@(hfs>=h|{7Ei0u%t^3d3bRU{-EpBt! z0h8sYnjYFEm$~ZM9eBczd)X}Kp08RANUgf*dzR||CGYo10Z9sKL&GSI!qt()LATG<}=AK}BKJ*>) z+X%$*MY!AUhq|oal+=IFC_4`j_<#B-XsmQu%nl+)m5Ha)#i;-!VEWgt&%as4F%v5} zIlagrFeJ=rm3k}9WfsV#C+WF@Xg@PNAYaB0%)f(l0n2_bPQ}x&rr?TYFUuN!fZ(t| zIctB{hiSjB;`~oI_6kV^4QoikBxF6sF+juso_H$S7Qz{HS105q%_}Roj?77{>;wL* zcyxL{ahfvN?&%zwaVr#*29H$y1`D|I;HN^z6Y%fUFX$c(r~{F8}aS)-}AlddBeV;7NIuVyYJO$&84CJ zMExu%B5;;SrfCo-80fUu7W|#~MKW*+6SzDE9G&znVCD`)@k;ko9Ik1tcp)f2o|y*u zBUW(Isjs*ijtrXs_kIr%x8;)eeK;&*Fjua}JR{P;3vCwm#VttBKzwJz(dhhF-eWBI zbWkxHG9}k?OpI#hn1<+oRI3#rPA}tB(g)SfcvM_stbb<16DJF!Kp`1k8GoDm-1Ou9 z9f>>X&M_|cjtknx#@rRK(5*#D7PZn&BI*)i{)!D&9fll zJwctzfysr+D~dn>}+$6%R-hj z(UdS846cc$rfre(}5N;J%^q0$c*8qxPJgx61EaLy3v-9Nev;Lc5G%Xr9sohV#*snmUc z;~ShoAE#9CPgZs%Y=}_d4g}xdq+7z~wI@1bG*&Od#`k(=^annaV(6)?q&qFHAs%~T+xXy+ zELAiC;j9$MRAv7g#}c2f4|Tb%A6r|icHb9wH@PMl3n3LqQpl)9;~?O|-_ziVfAGFZ zcHiP;>=^LA%y(Q>N=@ISe@=hWFO$|4pO)S`zeGk94#&nkz#vT!T_gtFV{~()lT+U2 zXrP+&Fjl}-r5hT5EcSWv`qXEBrg^Z^7w3pW&Z}m(HBH+Ub1q>%>6;oI!(kpy&SBD< zs0@jI`m8y~<(2tHrAwEU^7-jQ!^!;5gPF=L9mu*@7{SeM-V)AdkP^$jR|HAAAIAMl zG#k9U1fWKfquD*G<~0gI4Y}%61gaPFsF*1r%p3Ph{Wd;$cvY90@+V?lh=bIi#I3O# z4c{>tKJ&RrzrqAhm!o-Q7X4)6;=HMYULU9=u$=UX>827Fy#j5@=gomcl<}??j+K#H z6;_L1t6)i9p`G3V-%su{na$>Eq%KQ4SKHsEDS&pr-jr%fIc77g` zqg)_b3!hcZl&i|qm&v`|8O0&r=RwZuJLm~A=oc?uoWlbJk&qFxK~GT8PW!ovGlJC3}#VE zsssi9vrpK`Im{2g^M`Avcax?%#w_hk?kuw&JiVn~2R8TqQr#-77Mrr>-j;-oIek5Y zeCA26+uFL#JM(jc*N0x=7lZ>3Vbq&#G869ovk$c#%C^4SZ2Tx-^C3&61bE|gQpZW* z9q$k_%|^rM(G%5DlP{g46NdHqXnglsw9RQijrRC`5F0-N$qb`xpozuXf#!4YIN}kE z>V<)L&eu$IAWu(HW)fPKUc(@RMV#q<0Pmj-CrNe8UfLVkt>g2`8u+n+1#3GD3Hwr& zrHG$=?x!Wjgr6t#9n}X1n+2Rc|FmpIWus`H&_NIQNwbY5qd5`DeZj2BL<9bx%&`?oAweJJX$54>z8TwGEp+)EH zi>kS-cVMj{dcLW)ID9CD8=3yr2GBBC81w`<8g#GEK?+2TU-cvXXR=b_l3%yfx4r_C z*VarRxLJ?09g_0np%MP1j=CB9V6uR>ic2q<=oUBTQmb3Ztvh=^3xdvO@dij^==wB&XDCh_%lhut9yIx*E?OZR?C}ZC~sfNcK~L zV(a-gEVsQ|4QYxmue3bmX&D23!_JqZxha=kdjq(F?VQG!G^dzLBOhHU(NuNa=68^y#$HYL8p(8YDx^A5|Rjubl9TAdTz^j^UR@OI-)DIXnm~31|na zBZ|&*B7rX^T^_Zz>Y*|oAgrLhb(|sg73FN5WAexTXq8TQ)2^({Y34Fx!JQH&*l%tT zzCs6@x^YPRIVhTCzoZT>%Yd-Gyos>oglU=b`fq`kUtm)`+H)dWxkCTEz;kR>j#gY% zx8B7edc_FhFk_{v_Je)H{q$G!^d!#Bn)dH6&9|i!QnI%A0xxNa`Gin(@$o(xXMei4 zy7Foe<0l5Afz^_M+RsNkzM{>Knm<_jHBYFhDXLd zJmqM0lVKdYfql2K(+H?v*4l@Ud0Q6{nf+w9=S=jvMD`YLNj_2t6H#Qo6Afez1As}9T@pQVoV^^OBWJTpxsd?vqQf7U7 zFpVS2mz@;p*0lEcC}@db`*^L7{>S02gGRwc?WfH7uz^!Of-aWWj*0ubhr2xx&LP#^ zx^a4~IwfbST31&$x8#)oi*g&XV&pZsN=tGHWm{mjY~mwItMnZcSdFXbe*cIqQLB_| zsXT{|d z&9bN}73a4z1A`*%2n{>sPpadk#`>wi_76T@W^OJp`3%Fi({j2az5q`4spodVsuuM^0$D6Sqwi%0ArTM>q)Ns@dGre-T z7X%v7F$6k2)zl5RlM+ec90VB%eB> zTp!QOrb@aLy*Np;;8?Wj8N`vDbRQ0Guv^{fFq!sBT3E5;{|s9wlJQP=;LMvgpfcuE z{JdO5T9mfwQ8p8$T?d~x-ykfzVBw>>Z;tk@7aHI7jeFoN0ih4gjX9aAGkjEO$F?TI ziJuMcji+#01=k*{R@s@h#%#AZEmbU=fK5PQ9MQ!@0TL@m1bu-^YC({K$@6Y)zQjbq@Ri!&P|oJ>w043U&!AKe{(n=VcD#0 zu|kEeCw<%Lc8no@gw;j$ zW~7cWhwIi!C8iU@RYSK^YdbH$C9VM9C^LD+mG`S(NMIgJxn8&Rin;;whIUK*KT(8v3;&rpoA8pxgW}o z+Dw*P+FhJ3E3WRm?ai^_iKa_CCL1Xsen^^m6p*?qCE%RI?%^$R&%6pi<2gTDtFPxtWbB@7lcc<`|)!g)kJbNtxr3b z1YZ1?j|Bg0I9Iz4Oi75TNgYx+>14kWdn6e(j<{Q{!CZIa%%DEdcW%MmkEhZVPsViK z#Wr&Ng!l_|b+qN;exUlqd`e9M7UulOe1ZF7;mWJ|m9s%eUp=YRaT>Cqo6|U=Wx&$$ znDx@~>TC1s{X&++v-){lK&5_YP2CAR*e+cnosnZF26?k_Tg~9+jPCJz+9;yb1aQ zch3x>E>C`T%&)q*kf;55LUcAgPr>s&+(hgIV_7oS5@h+p668DN&i)(Bb#tx+*q?T7 zVxy0_>ZoZsN}zLgO;gi)vftuYF_eN1Yv$FL{aRpQ`wH4_4EvSqMG=Fef^x9NGJW6r zY4peDZV$g?bVfnhc-y0TFfEQhhPvMRbpjg*Angj(I7k$bPA+NE!;=Q>LO1gZ)pdvS z)OpvZYRa&QxKbdm#Y;m+_l7!>2^ATN7n?f17x3O6ZIGtt>XL31e6U$jJeA{Ev{8JK z{$Vv3QA859qJ)`5sF|x&^YFez$Jx@c6q(4O;_4J7JR^DGBlb>WLP7#!cRTGrcW`wZ z(N!L0$anUQy)Qg-af=yv%OY>Uirb8}N6uT=ZK?N^+~4-);APVbkULHH;#H;d8t`?4 zsKV&eq6`xkKhd)Jbm5qIspzIp27iJ!ZLPVxCd}e-m;H&j50jwSen4U`jsez8e{E*6 z)za}w-*=VAUEM%dV6}6Gv->{5KXaLy=_#Ciz_d@y@2h7OBrrAUH4HQjIparcrkZVE z;cCRG;kovrUplOgXBt~68+{jo$;9(d*|>%E)sc~=WnND5ubEZtH}iI-w;ytA?Ri_M zxkppjk}v&vMm!;kDxqr9gi$pRt)SBFjta7G1+|>7&bbPP&)J39Sk{(S&^tYV#$m-V zJBxlgE&+r8X&;&NlI<2^Kzlr!;nza|Yq!#Iv$SO9-j8XG=+OP=K<>pu(rymGXn^gW zw)lvdvQNd#A=dF+!i$J+oNWlAKi(P9uAw{Y6CynzjI-B5Ux7~=$!6ed9Il4U(4ZWn z9OZq#^Y7*b;;|Zh*__;N$EL*+Q)CB|c*bemT^O^g@HZ&V*&vQ5QR1*!GC~J{&R0tJvU;e}-6;DuF`nS+QvI z58g?F$>YyRmISDyhYIIv)9AeggbWaQD)V$N7t?f}-p?8ud-u`)x+x~q!Lcn<1ad>@ zNnKunyHQnR(5lQDU9I9bXj1pN!}JA{aT5(nbhroDpo;WmjG2KGurIsuuy^-)n(b#b z?B8JkTc}4+-tL#f-4@dt>FHUh;|wGoUZ9FQ?y4No9Y*WW&wjJm{k%XWb?CLGd*2Y@ z2hr#1qBmEo?_7tg;GQy~Jl^d&oNqI`RhG|ayOAqH9h+d81+{^R=^o$D3`kOkb`g0q zWfq5X!c9I1&EJvLcRSviVt`t{jaQ-kCxQ}U#B{LO8W>FAQ=OWBR_FefnDp{ly%>Du z-pvd4HL>Nsm!zo2?Xjl^De0#0%&-dBYHLqg7tddJu_ji}J84_IHs>C+&)yDcns*I4 zbkz)c=ZZbJ>&Yvgob0;NifeK{;E!^3VqQ5x8rp@jUVfEeEO`E~&mX;9{`Z?1EJ6M2WGYb{4Pgi+|n?5#;`Q?`O=ovdqLV#Dm*|@U&vsNhBbk%m0p)A|PV+!}q!IsLp{o!+e)(d! zY9XZW17SZx>&?*j^q4nqTIIi$$GDztn(i2Q^V>|TaC@?M=i{m}Q7pg0M8~y0#chG> zW8UB<62JgzjCaUVCdkax8jch^_`zS#CB%XF?%aeY$9N5Ctx7TL9K5o<+oj7u z%kM5j^jV3u>+j3-dZUJl`t%)3onS8Y{|O4BV-c9$ieT*(KkpzOj%8m8oS*XW?5TUv z{u>kjb*bNRJvLJU+v!Eb&K+I>)04H=$bU_^Kkxg`cfFrn!k!9iT%M4#{q?k>nxJl3 z>IOe^wcb%7vju*(M{P9mgI3V27+h|c2@D7!Wi#e{S92Xq#YxvC{OkRXR6Onp@;_j9 zbBd@@&+0qX(y0}Y{d3Wy*n0QNi(?MEjZs&7FO~bJ0a2`*(YGj#3R>b~7*$IZjM(Q^Z* zg)d?%0EqJaOcG|enEjeG-uXz`*!`y`$AheW;N*K}E!p;k_43Dkzy}urSjpniX_tK| zdIp9P(D41}sNV7njr{(aoVxljR8^nf=>Q$Ji+eqXg!b=HG($}|S0;MyWGfOX>W|B` zJZg>*HX|%yxIFD~|9E6_FRG`*+!}_NZ1;PRWt|aGi?1A&NblM58UJjgXR1?6et$j} z=e)5LV|Fukw`|(VZ_0K#whdlLx)0F7g&~+XH&QbwH3W$#FDGWk!F>s*!>DcN#7M_e z`>F-p$%a}wGG^f29A=XSn-gVaGl07YUuY3LQqVdZFou@UL#tVA8rSbJNyZX~Ymk^8 zx1f<@3391f?KatUv5!1Dj^QwkjVT#mcQv5w7f^fs8O~FgY=0nvobLeBI6xT4*B=Q?{Zg#Lug>cic(%hu) z&(IWHD&M_O5?Gq`p~u-8Wv-B_!K*{podS6S+m|_p?ZXU z!cC-`zijT3%$;vzf9w2|;5D1_?vdFx{YPAAx%*(o!Y+^fmf9`=uxPoh^d=IxyR0NG zRL;YWavy&a;OCCdWXuYwmAQ_&J%sk}Ud^Q$9ls$0UVY!}qD?XX8Lo>N-y0eVlHa%r z=74utE;_VvsP#N!t>U-{nf><$3NoO_c(yq{U$&5I4;e^oFX5$B!~qa>WzerBAS6_c zpAW)~A#%9u8ALx}?cpn`l-nq{7}Jj{u+JMe(G4SK*s2bbZYtxyTlWOTZ8B>@igvFr{OC(iD(jYjd3t)=mzTlBFq zmn&_^VJm1sbhLh<4_6lCq+bosuG7TdCmWGs{E2+MY7mePDS{cudTboPu4Ib*?p)Gd za*IK+&h_CJbQ#sC2zUG3>_S@GZ`|0^H;t?aQ-~ z95gCY)U50jJ$q+nvP>znpEW1@@<0JY1K3| zkS3?!8M!)xv3-|(TW6%w1cuEsj}^nBE6(K&d6!MN@Sw%t$izZUyDq_Mf(Q9T8L+w=NnV6`h&C|J3P#-3rw6eLgo zsZRCEqp;1vf8a}_Gr14>y)zGl=&6RVusEG=25?T|9n~;8oo+~R&U~9>m|!+9-xXaS z%Z^wiw+*E&34$PH<2W&iBsto89Q*O~*I92>Xjul4@tch1YuWXGrun#k>KEV=Z!+M# zUDab&x5>dPAdY%xv7-!GrA*q4Y!O-(M95T1nS|p$w739up9+7X{8bIwcli3kotH{w z`qEfK!Y*Hb!W;-U9*xwUH=P}=;zO#l`%WOWm+#3(fhO&D9@6c~b!gdRP_N0o^UN=a z2B&z&wQm4PybqrVjL6K#o_{}D5N5mt+h3g2r+b2~6X3v~P!Hir>Y_0V3(AUTNDGL6`XghFVXn9W>Xm&6mksJOtiTV93QX>4Z36&0fAvnx#XQhbDZfU&K_TltWwU>*hpO_hbQk7TiQ@zIj zcPdiE)R&>*mNps=2-Vhyco-<&(pcX3pen(-@C%)HzFaE6YOn|6G;{(eou>eWSJx4+V7)_9z4@()SJeOzXx zt2_F%;rTmst)P*Wl?`?dOS;}iu6`k_51xo;pg{hz=8wetn5A`yo{H+W`i%FYr{ zqyOi%&Cng*#)6i)awG4qBZiro3-=(>bw=|44I?3Vm|#{7{3+O&-hSZ{(EA@5;D0@ukQw@8{@OPi8ISlBYnfUD z=-y!dJI(&T8%2pO^Qc3xw@-I#ycaf8ZH}Ht_J2JU#WBj@XXqW?weK<+`th$%D~_pN zQ~qCX?775{aN*2^ak%1~MXJ&ISN-DudwXI?!tbn0r#qdsZnh53=&y6>zn@J_(>~9I zo{0&%(Ww|s%5yOP+(q>Nf2!YeGeHZLPpbB8=Xk0}*x&V@|9YX2;U1hR z@z&{DtG<`@op|R3K`>YpI zf3Md6dhB{swtE_>3fXTp?3Q4{GoSwFWc>z~H~#16FU#;nmqLuvXKS&#|H<6{exQHv zONa;2c42$GgueQiBQemWrz*ruJN)kq`oCTz^c}s}pzoo7cM#7JexjmA4eWmoE;izJ z*@(w^uwIMvk#(}u|5}hJ;1S`+YlAWlwWLb_V=O4_Q9`L_E`_5>6{Il#=iWhbwRZ>} z&d?k@U}9q8G!kX_UvG<^fjFB-Fpt4DR#g8 zW1rpEzn9MbHj>re7hAJ){_*R-YhH!?E~xwS_uj{q_sw5EtDX67>5sCHPwVFYteWpX zH%tDV{X6^p@{%g+B*I~inITvxt${JZZn*MI+c zW#zYr{~rJQTz!0b-TfuAzo+h+{^<76>A!2Ay{@$@T~)i||BnBE28os5SGqk~9bfnG qP-|F3L Date: Wed, 24 Feb 2021 16:43:42 +0100 Subject: [PATCH 114/749] reanmed current to dc_current --- README.md | 3 ++- .../current_control/current_control.ino | 2 +- .../inline_current_sense_test.ino | 2 +- .../stepper_driver_2pwm_standalone.ino | 3 +++ keywords.txt | 4 +-- src/BLDCMotor.cpp | 9 ++++--- src/common/base_classes/CurrentSense.cpp | 2 +- src/common/base_classes/CurrentSense.h | 26 +++++++++---------- src/common/base_classes/FOCMotor.h | 2 +- src/communication/Commander.cpp | 8 +++--- 10 files changed, 33 insertions(+), 28 deletions(-) diff --git a/README.md b/README.md index d826f783..7e403715 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ Therefore this is an attempt to: - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop a modular FOC supporting BLDC driver boards: - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - ***NEW***: *Medium-power* BLDC driver (<50Amps): [*Arduino Simple**FOC**PowerShield*](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). + - ***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 📢: SimpleFOClibrary v2.1 > #### Implemented features in dev branch diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index e4d8ba68..3242f0cd 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -46,7 +46,7 @@ void setup() { motor.linkCurrentSense(¤t_sense); // set torque mode: - // TorqueControlType::current + // TorqueControlType::dc_current // TorqueControlType::voltage // TorqueControlType::foc_current motor.torque_controller = TorqueControlType::foc_current; diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 5d8dd9b2..798273c1 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -24,7 +24,7 @@ void setup() { void loop() { PhaseCurrent_s currents = current_sense.getPhaseCurrents(); - float current_magnitude = current_sense.getCurrent(); + float current_magnitude = current_sense.getDCCurrent(); Serial.print(currents.a*1000); // milli Amps Serial.print("\t"); diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 0e9a0189..7289ac79 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -6,6 +6,9 @@ // StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); +// StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional)) +// StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12); + void setup() { // pwm frequency to be used [Hz] diff --git a/keywords.txt b/keywords.txt index 35b42752..cfec8b76 100644 --- a/keywords.txt +++ b/keywords.txt @@ -78,7 +78,7 @@ angleOpenloop KEYWORD2 velocityOpenloop KEYWORD2 getPhaseCurrents KEYWORD2 getFOCCurrents KEYWORD2 -getCurrent KEYWORD2 +getDCCurrent KEYWORD2 setPwm KEYWORD2 driverAlign KEYWORD2 driverSync KEYWORD2 @@ -124,7 +124,7 @@ velocity_openloop KEYWORD2 angle KEYWORD2 angle_openloop KEYWORD2 torque KEYWORD2 -current KEYWORD2 +dc_current KEYWORD2 foc_current KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index ab38df38..dd939e6c 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -183,8 +183,9 @@ int BLDCMotor::alignSensor() { // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.25 ) { // 0.25 is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->println(F("fail!")); + if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if(monitor_port) monitor_port->print(F("fail - estimated pp:")); + if(monitor_port) monitor_port->println(_2PI/moved,4); return 0; // failed calibration }else if(monitor_port) monitor_port->println(F("OK!")); @@ -250,10 +251,10 @@ void BLDCMotor::loopFOC() { case TorqueControlType::voltage: // no need to do anything really break; - case TorqueControlType::current: + case TorqueControlType::dc_current: if(!current_sense) return; // read overall current magnitude - current.q = current_sense->getCurrent(electrical_angle); + current.q = current_sense->getDCCurrent(electrical_angle); // filter the value values current.q = LPF_current_q(current.q); // calculate the phase voltage diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 57d48025..accaebb3 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -4,7 +4,7 @@ // get current magnitude // - absolute - if no electrical_angle provided // - signed - if angle provided -float CurrentSense::getCurrent(float motor_electrical_angle){ +float CurrentSense::getDCCurrent(float motor_electrical_angle){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); // currnet sign - if motor angle not provided the magnitude is always positive diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 1b8a79af..0904dea2 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -24,6 +24,18 @@ class CurrentSense{ */ virtual int driverSync(BLDCDriver *driver) = 0; + // calibration variables + bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() + /** + * Function intended to verify if: + * - phase current are oriented properly + * - if their order is the same as driver phases + * + * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) + * @returns - 0 - for failure & positive number (with status) - for success + */ + virtual int driverAlign(BLDCDriver *driver, float voltage) = 0; + /** * Function rading the phase currents a, b and c * This function will be used with the foc control throught the function @@ -41,7 +53,7 @@ class CurrentSense{ * * @param angle_el - electrical angle of the motor (optional) */ - virtual float getCurrent(float angle_el = 0); + virtual float getDCCurrent(float angle_el = 0); /** * Function used for FOC contorl, it reads the DQ currents of the motor @@ -50,18 +62,6 @@ class CurrentSense{ * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); - - // calibration variables - bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - /** - * Function intended to verify if: - * - phase current are oriented properly - * - if their order is the same as driver phases - * - * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & positive number (with status) - for success - */ - virtual int driverAlign(BLDCDriver *driver, float voltage) = 0; }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 450128b3..357063b0 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -37,7 +37,7 @@ enum MotionControlType{ */ enum TorqueControlType{ voltage, //!< Torque control using voltage - current, //!< Torque control using current + dc_current, //!< Torque control using DC current (one current magnitude) foc_current //!< torque control using dq currents }; diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 97a76bb3..bdd1b509 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -202,13 +202,13 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ case TorqueControlType::voltage: - println(F("volt")); + println(F("dc volt")); break; - case TorqueControlType::current: - println(F("curr")); + case TorqueControlType::dc_current: + println(F("dc curr")); break; case TorqueControlType::foc_current: - println(F("foc")); + println(F("foc curr")); break; } break; From 271a0b66c4ce4b94c08130ad9ff0bfbad1d64463 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 24 Feb 2021 23:22:17 +0100 Subject: [PATCH 115/749] FEAT if added phase resistance the torque control is with current --- src/BLDCMotor.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index dd939e6c..23874a2c 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -28,7 +28,7 @@ void BLDCMotor::init() { // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && _isset(phase_resistance)) { - float new_voltage_limit = current_limit * (phase_resistance*1.5); // v_lim = current_lim / (3/2 phase resistance) - worst case + float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case // use it if it is less then voltage_limit set by the user voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; } @@ -300,8 +300,9 @@ void BLDCMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage) - voltage.q = target; // if voltage torque control + if(torque_controller == TorqueControlType::voltage) // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = target*phase_resistance; else current_sp = target; // if current/foc_current torque control break; @@ -316,7 +317,7 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*1.5*phase_resistance; + else voltage.q = current_sp*phase_resistance; voltage.d = 0; } break; @@ -329,7 +330,7 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*1.5*phase_resistance; + else voltage.q = current_sp*phase_resistance; voltage.d = 0; } break; From 88099223a69d288d3c4767a731728cd3fd6c6f75 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 25 Feb 2021 02:09:50 +0100 Subject: [PATCH 116/749] removed pp requirement + current_limit used in open loop --- src/BLDCMotor.cpp | 38 +++++++++++++++++++++++++++----------- src/BLDCMotor.h | 4 ++-- 2 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 23874a2c..7b8b54c6 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -186,7 +186,6 @@ int BLDCMotor::alignSensor() { if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); - return 0; // failed calibration }else if(monitor_port) monitor_port->println(F("OK!")); }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); @@ -216,16 +215,22 @@ int BLDCMotor::absoluteZeroSearch() { if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity - float limit = velocity_limit; + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ angleOpenloop(1.5*_2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->getAngle(); } // disable motor setPhaseVoltage(0, 0, 0); // reinit the limits - velocity_limit = limit; + velocity_limit = limit_vel; + voltage_limit = limit_volt; // check if the zero found if(monitor_port){ if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); @@ -337,15 +342,13 @@ void BLDCMotor::move(float new_target) { case MotionControlType::velocity_openloop: // velocity control in open loop shaft_velocity_sp = target; - velocityOpenloop(shaft_velocity_sp); - voltage.q = voltage_limit; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor voltage.d = 0; break; case MotionControlType::angle_openloop: // angle control in open loop shaft_angle_sp = target; - angleOpenloop(shaft_angle_sp); - voltage.q = voltage_limit; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor voltage.d = 0; break; } @@ -553,7 +556,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Function (iterative) generating open loop movement for target velocity // - target_velocity - rad/s // it uses voltage_limit variable -void BLDCMotor::velocityOpenloop(float target_velocity){ +float BLDCMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call @@ -565,18 +568,24 @@ void BLDCMotor::velocityOpenloop(float target_velocity){ shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; + + return Uq; } // Function (iterative) generating open loop movement towards the target angle // - target_angle - rad // it uses voltage_limit and velocity_limit variables -void BLDCMotor::angleOpenloop(float target_angle){ +float BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call @@ -593,9 +602,16 @@ void BLDCMotor::angleOpenloop(float target_angle){ shaft_angle = target_angle; shaft_velocity = 0; } + + + // use voltage limit or current limit + float Uq = voltage_limit; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; + + return Uq; } diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 761c1ebb..e5947590 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -96,14 +96,14 @@ class BLDCMotor: public FOCMotor * * @param target_velocity - rad/s */ - void velocityOpenloop(float target_velocity); + float velocityOpenloop(float target_velocity); /** * Function (iterative) generating open loop movement towards the target angle * it uses voltage_limit and velocity_limit variables * * @param target_angle - rad */ - void angleOpenloop(float target_angle); + float angleOpenloop(float target_angle); // open loop variables long open_loop_timestamp; }; From d80497116b8f8c7fdcba8f743cc5a67b0bd3a1b2 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 3 Mar 2021 11:46:30 +0100 Subject: [PATCH 117/749] FEAT changed step-dir constructor form step2rotation to count2value --- .../step_dir_listener_simple/step_dir_listener_simple.ino | 4 ++-- .../step_dir_listener_software_interrupt.ino | 4 ++-- .../step_dir_motor_example/step_dir_motor_example.ino | 4 ++-- src/communication/StepDirListener.cpp | 6 +++--- src/communication/StepDirListener.h | 6 +++--- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino b/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino index 0c9bf440..a0bebc35 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_simple/step_dir_listener_simple.ino @@ -8,8 +8,8 @@ // angle float received_angle = 0; -// StepDirListener( step_pin, dir_pin, counts_per_revolution) -StepDirListener step_dir = StepDirListener(2, 3, 200); +// StepDirListener( step_pin, dir_pin, counter_to_value) +StepDirListener step_dir = StepDirListener(2, 3, 360.0/200.0); // receive the angle in degrees void onStep() { step_dir.handle(); } void setup() { diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino index d1edaa0d..d723313c 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -13,8 +13,8 @@ // angle float received_angle = 0; -// StepDirListener( step_pin, dir_pin, counts_per_revolution) -StepDirListener step_dir = StepDirListener(4, 5, 200); +// StepDirListener( step_pin, dir_pin, counter_to_value) +StepDirListener step_dir = StepDirListener(4, 5, 2.0*_PI/200.0); // receive the angle in radians void onStep() { step_dir.handle(); } // If no available hadware interrupt pins use the software interrupt diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index 804c7753..e46e80e6 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -18,8 +18,8 @@ Encoder encoder = Encoder(2, 3, 500); void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } -// StepDirListener( step_pin, dir_pin, counts_per_revolution) -StepDirListener step_dir = StepDirListener(A4, A5, 200); +// StepDirListener( step_pin, dir_pin, counter_to_value) +StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200); void onStep() { step_dir.handle(); } void setup() { diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index 34a541aa..c8f19b47 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -1,9 +1,9 @@ #include "StepDirListener.h" -StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _step_per_rotation){ +StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _counter_to_value){ pin_step = _pinStep; pin_dir = _pinDir; - step_per_rotation = _step_per_rotation; + counter_to_value = _counter_to_value; } void StepDirListener::init(){ @@ -36,5 +36,5 @@ void StepDirListener::handle(){ } // calculate the position from counter float StepDirListener::getValue(){ - return (float) count / step_per_rotation * _2PI; + return (float) count * counter_to_value; } \ No newline at end of file diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 7674600c..1c50c700 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -15,9 +15,9 @@ class StepDirListener * Constructor for step/direction interface * @param step - pin * @param direction - pin - * @param step_per_rotation - number of steps per motor rotation + * @param counter_to_value - step counter to value */ - StepDirListener(int pinStep, int pinDir, float step_per_rotation = 1); + StepDirListener(int pinStep, int pinDir, float counter_to_value = 1); /** * Start listenning for step commands * @@ -50,7 +50,7 @@ class StepDirListener private: float* attached_variable = nullptr; //!< pointer to the attached variable - long step_per_rotation; //!< number of steps per rotation + float counter_to_value; //!< step counter to value bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable }; From cfa2dc6e6a365da8c8b97893a31c12606e339a8d Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 3 Mar 2021 15:27:32 +0100 Subject: [PATCH 118/749] renamed varible to scalar because it has to be a number --- .../bluepill_position_control.ino | 2 +- .../bluepill_position_control.ino | 2 +- .../esp32_position_control/esp32_position_control.ino | 2 +- .../esp32_position_control/esp32_position_control.ino | 2 +- .../position_control/position_control.ino | 2 +- .../HMBGC_example/voltage_control/voltage_control.ino | 2 +- .../open_loop_position_example.ino | 2 +- .../open_loop_velocity_example.ino | 2 +- .../encoder/angle_control/angle_control.ino | 2 +- .../hall_sensor/angle_control/angle_control.ino | 2 +- .../magnetic_sensor/angle_control/angle_control.ino | 2 +- .../encoder/current_control/current_control.ino | 2 +- .../encoder/voltage_control/voltage_control.ino | 2 +- .../hall_sensor/voltage_control/voltage_control.ino | 2 +- .../voltage_control/voltage_control.ino | 2 +- .../encoder/velocity_control/velocity_control.ino | 2 +- .../hall_sensor/velocity_control.ino | 2 +- .../velocity_control/velocity_control.ino | 2 +- .../commander_tune_custom_loop.ino | 2 +- .../step_dir_motor_example/step_dir_motor_example.ino | 2 +- src/communication/Commander.cpp | 10 ++++++++-- src/communication/Commander.h | 4 ++-- 22 files changed, 30 insertions(+), 24 deletions(-) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 4eb36aa5..267c00fb 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -28,7 +28,7 @@ void doI(){encoder.handleIndex();} float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { 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 434b2b5c..54608e8a 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 @@ -31,7 +31,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 4e09d007..9864ca0b 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -20,7 +20,7 @@ void doB(){encoder.handleB();} float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 2a9e991d..4bd53b55 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -29,7 +29,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 372d4cd1..e9355f65 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -38,7 +38,7 @@ PciListenerImp listenerB(encoder.pinB, doB); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino index 08c6f17a..9c9655f9 100644 --- a/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/voltage_control/voltage_control.ino @@ -44,7 +44,7 @@ PciListenerImp listenerB(encoder.pinB, doB); float target_voltage = 2; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 1e728e9a..a4756352 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -17,7 +17,7 @@ float target_position = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_position, cmd); } +void doTarget(char* cmd) { command.scalar(&target_position, cmd); } void setup() { diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 0ade034f..f7a82eb3 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -18,7 +18,7 @@ float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 3bdedcc4..a8e8b997 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -50,7 +50,7 @@ PciListenerImp listenerIndex(encoder.index_pin, doIndex); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 77bc5380..90219b5b 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -43,7 +43,7 @@ PciListenerImp listenC(sensor.pinC, doC); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index a600ed43..d22289bd 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -27,7 +27,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_angle, cmd); } +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index 3242f0cd..ad75b573 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -23,7 +23,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); float target_current = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_current, cmd); } +void doTarget(char* cmd) { command.scalar(&target_current, cmd); } void setup() { diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 2cac0214..66ff4569 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -29,7 +29,7 @@ void doB(){encoder.handleB();} float target_voltage = 2; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 89d47c78..7273d156 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -31,7 +31,7 @@ void doC(){sensor.handleC();} float target_voltage = 2; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index b60f0018..d869f4dd 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -26,7 +26,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); float target_voltage = 2; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_voltage, cmd); } +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index f77023b4..a3974b38 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -52,7 +52,7 @@ PciListenerImp listenerIndex(encoder.index_pin, doIndex); float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 404a7810..07b70dcd 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -43,7 +43,7 @@ PciListenerImp listenerIndex(sensor.pinC, doC); float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 58d02220..a16385df 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -29,7 +29,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino index e8630eb5..cbeb6ecb 100644 --- a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino +++ b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -24,7 +24,7 @@ LowPassFilter LPFv{0.01}; Commander command = Commander(Serial); void doController(char* cmd) { command.pid(&PIDv, cmd); } void doFilter(char* cmd) { command.lpf(&LPFv, cmd); } -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index e46e80e6..932edc81 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -19,7 +19,7 @@ void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200); +StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200.0); void onStep() { step_dir.handle(); } void setup() { diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index bdd1b509..1307b16e 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -35,7 +35,11 @@ void Commander::run(){ } } -void Commander::run(HardwareSerial &serial){ +void Commander::run(HardwareSerial& serial){ + HardwareSerial* 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; + // a string to hold incoming data while (serial.available()) { // get the new byte: @@ -50,6 +54,8 @@ void Commander::run(HardwareSerial &serial){ rec_cnt=0; } } + + com_port = tmp; // reset the instance to the internal value } void Commander::run(char* user_input){ @@ -372,7 +378,7 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ } } -void Commander::variable(float* value, char* user_cmd){ +void Commander::scalar(float* value, char* user_cmd){ bool GET = user_cmd[0] == '\n'; if(!GET) *value = atof(user_cmd); println(*value); diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 03657094..53cf9774 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -159,12 +159,12 @@ class Commander */ void pid(PIDController* pid, char* user_cmd); /** - * Float variable command interface + * Float variable scalar command interface * - It only has one property - one float value * - It can be get by sending an empty string '\n' * - It can be set by sending 'value' - (ex. 0.01 for settin *value=0.01) */ - void variable(float* value, char* user_cmd); + void scalar(float* value, char* user_cmd); private: // Subscribed command callback variables From 10759a0f5dda3f2555a0de5c72474061680a95fb Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Mar 2021 19:52:17 +0100 Subject: [PATCH 119/749] Tested steppers, added support for phase resitance --- .github/workflows/ccpp.yml | 2 +- README.md | 4 +- .../single_full_control_example.ino | 2 +- .../full_control_serial.ino | 28 +-- .../full_control_serial.ino | 28 +-- .../full_control_serial.ino | 28 +-- ...lcontrol.ino => osc_esp32_fullcontrol.ino} | 0 src/BLDCMotor.cpp | 4 +- src/StepperMotor.cpp | 222 ++++++++++++------ src/StepperMotor.h | 13 +- src/common/base_classes/BLDCDriver.h | 2 +- src/common/base_classes/FOCMotor.h | 4 +- src/common/defaults.h | 4 + src/communication/Commander.cpp | 10 +- src/communication/commands.h | 5 +- src/current_sense/InlineCurrentSense.cpp | 9 +- src/drivers/StepperDriver4PWM.cpp | 2 +- 17 files changed, 188 insertions(+), 179 deletions(-) rename examples/osc_control_examples/osc_esp32_fullcontrol/{simplefoc_osc_esp32_fullcontrol.ino => osc_esp32_fullcontrol.ino} (100%) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 71490d53..27a86e9e 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, simplefoc_osc_esp32_3pwm + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol diff --git a/README.md b/README.md index 7e403715..7c783522 100644 --- a/README.md +++ b/README.md @@ -53,7 +53,7 @@ Therefore this is an attempt to: > - `ControlType::voltage` does not exist any more now - `MotionControlType::torque` -## Arduino *SimpleFOCShield* v2.0.2 +## Arduino *SimpleFOCShield* v2.0.3

    -## Arduino *SimpleFOClibrary* v2.0.2 +## Arduino *SimpleFOClibrary* v2.1

    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 4e38141f..89bbade3 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 @@ -72,7 +72,7 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); _delay(1000); diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 1f3bea03..30d18297 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -8,33 +8,7 @@ * - set target values * - check all the configuration values * - * To check the config value just enter the command letter. - * For example: - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PID controller P gain - * - I: velocity PID controller I gain - * - D: velocity PID controller D gain - * - R: velocity PID controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - L: system voltage limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: angle - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * + * See more info in docs.simplefoc.com/commander_interface */ #include diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index e62859f0..7a202f94 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -8,33 +8,7 @@ * - set target values * - check all the configuration values * - * To check the config value just enter the command letter. - * For example: - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PID controller P gain - * - I: velocity PID controller I gain - * - D: velocity PID controller D gain - * - R: velocity PID controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - L: system voltage limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: angle - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * + * See more info in docs.simplefoc.com/commander_interface */ #include // software interrupt library diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index ae41499d..97216a9e 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -8,33 +8,7 @@ * - set target values * - check all the configuration values * - * To check the config value just enter the command letter. - * For example: - to read velocity PI controller P gain run: P - * - to set velocity PI controller P gain to 1.2 run: P1.2 - * - * To change the target value just enter a number in the terminal: - * For example: - to change the target value to -0.1453 enter: -0.1453 - * - to get the current target value enter: V3 - * - * List of commands: - * - P: velocity PID controller P gain - * - I: velocity PID controller I gain - * - D: velocity PID controller D gain - * - R: velocity PID controller voltage ramp - * - F: velocity Low pass filter time constant - * - K: angle P controller P gain - * - N: angle P controller velocity limit - * - L: system voltage limit - * - C: control loop - * - 0: voltage - * - 1: velocity - * - 2: angle - * - V: get motor variables - * - 0: currently set voltage - * - 1: current velocity - * - 2: current angle - * - 3: current target value - * + * See more info in docs.simplefoc.com/commander_interface */ #include diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino similarity index 100% rename from examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino rename to examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 7b8b54c6..545427cd 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,6 +1,6 @@ #include "BLDCMotor.h" -// BLDCMotor( int pp) +// BLDCMotor( int pp , float R) // - pp - pole pair number // - R - motor phase resistance BLDCMotor::BLDCMotor(int pp, float _R) @@ -306,7 +306,7 @@ void BLDCMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: if(torque_controller == TorqueControlType::voltage) // if voltage torque control - if(!_isset(phase_resistance)) voltage.q = current_sp; + if(!_isset(phase_resistance)) voltage.q = target; else voltage.q = target*phase_resistance; else current_sp = target; // if current/foc_current torque control diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 1c28fb1e..604914ed 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -2,11 +2,14 @@ // StepperMotor(int pp) // - pp - pole pair number -StepperMotor::StepperMotor(int pp) +// - R - motor phase resistance +StepperMotor::StepperMotor(int pp, float _R) : FOCMotor() { // number od pole pairs pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; // torque control type is voltage by default // current and foc_current not supported yet @@ -22,20 +25,31 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { // init hardware pins void StepperMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init variables.")); - + if(monitor_port) monitor_port->println(F("MOT: Init")); + + // if set the phase resistance of the motor use current limit to calculate the voltage limit + if(_isset(phase_resistance)) { + float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case + // use it if it is less then voltage_limit set by the user + voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; + } // sanity check for the voltage limit configuration if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; // update the controller limits - PID_velocity.limit = voltage_limit; + if(_isset(phase_resistance)){ + // velocity control loop controls current + PID_velocity.limit = current_limit; + }else{ + PID_velocity.limit = voltage_limit; + } P_angle.limit = velocity_limit; _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable.")); + if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); @@ -72,100 +86,142 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int exit_flag = 1; // align motor if necessary // alignment necessary for encoders! - if(!_isset(zero_electric_offset)){ + if(_isset(zero_electric_offset)){ // abosolute zero offset provided - no need to align zero_electric_angle = zero_electric_offset; // set the sensor direction - default CW sensor_direction = _sensor_direction; - }else{ - // sensor and motor alignment - _delay(500); - exit_flag = alignSensor(); - _delay(500); - } - if(monitor_port) monitor_port->println(F("MOT: Motor ready.")); + } + // 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(exit_flag){ + if(monitor_port) monitor_port->println(F("MOT: Ready.")); + }else{ + if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + disable(); + } + return exit_flag; } // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { + int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - // check if sensor needs zero search - if(sensor->needsSearch()) absoluteZeroSearch(); - _delay(500); + // if unknown natural direction + if(!_isset(sensor_direction)){ + // check if sensor needs zero search + if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); + // stop init if not found index + if(!exit_flag) return exit_flag; - // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees - float start_angle = shaftAngle(); - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - // move one electrical revolution forward - _delay(500); - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // take and angle in the middle - float mid_angle = shaftAngle(); - // move one electrical revolution forward - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // determine the direction the sensor moved - if (mid_angle < start_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); - sensor_direction = Direction::CCW; - } else if (mid_angle == start_angle) { - if(monitor_port) monitor_port->println(F("MOT: Sensor failed to notice movement")); - } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); - } + // find natural direction + // move one electrical revolution forward + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(2); + } + // take and angle in the middle + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0 ; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(2); + } + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) { + if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + return 0; // failed calibration + } else if (mid_angle < end_angle) { + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + sensor_direction = Direction::CCW; + } else{ + if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + sensor_direction = Direction::CW; + } + // check pole pair number + if(monitor_port) monitor_port->print(F("MOT: PP check: ")); + float moved = fabs(mid_angle - end_angle); + if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if(monitor_port) monitor_port->print(F("fail - estimated pp:")); + if(monitor_port) monitor_port->println(_2PI/moved,4); + }else if(monitor_port) monitor_port->println(F("OK!")); - // let the motor stabilize for 1 sec - _delay(1000); - // set sensor to zero - zero_electric_angle = _normalizeAngle(_electricalAngle(shaftAngle(), pole_pairs)); - _delay(500); - setPhaseVoltage(0, 0, 0); - _delay(200); - - return 0; + }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + + // zero electric angle not known + if(!_isset(zero_electric_angle)){ + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + _delay(20); + if(monitor_port){ + monitor_port->print(F("MOT: Zero elec. angle: ")); + monitor_port->println(zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + return exit_flag; } // Encoder alignment the absolute zero angle // - to the index -void StepperMotor::absoluteZeroSearch() { +int StepperMotor::absoluteZeroSearch() { - if(monitor_port) monitor_port->println(F("MOT: Absolute zero search...")); + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity - float limit = velocity_limit; + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ angleOpenloop(1.5*_2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->getAngle(); } // disable motor setPhaseVoltage(0, 0, 0); // reinit the limits - velocity_limit = limit; + velocity_limit = limit_vel; + voltage_limit = limit_volt; // check if the zero found if(monitor_port){ if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); else monitor_port->println(F("MOT: Success!")); } + return !sensor->needsSearch(); } + // 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(); - electrical_angle = _normalizeAngle(_electricalAngle(shaft_angle, pole_pairs) - zero_electric_angle); + electrical_angle = electricalAngle(); + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -180,6 +236,7 @@ void StepperMotor::move(float new_target) { if(!enabled) return; // downsampling (optional) if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; // set internal target variable if(_isset(new_target) ) target = new_target; // get angular velocity @@ -187,35 +244,45 @@ void StepperMotor::move(float new_target) { // choose control loop switch (controller) { case MotionControlType::torque: - voltage.q = target; + if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control + else voltage.q = target*phase_resistance; voltage.d = 0; break; case MotionControlType::angle: // angle set point - // include angle loop shaft_angle_sp = target; + // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = current_sp*phase_resistance; voltage.d = 0; break; case MotionControlType::velocity: // velocity set point - // include velocity loop shaft_velocity_sp = target; - voltage.q = PID_velocity(shaft_velocity_sp - shaft_velocity); + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = current_sp*phase_resistance; voltage.d = 0; break; case MotionControlType::velocity_openloop: // velocity control in open loop - // loopFOC should not be called shaft_velocity_sp = target; - velocityOpenloop(shaft_velocity_sp); + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; break; case MotionControlType::angle_openloop: // angle control in open loop - // loopFOC should not be called shaft_angle_sp = target; - angleOpenloop(shaft_angle_sp); + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; break; } } @@ -234,6 +301,7 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // angle normalization in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el); float _ca = _cos(angle_el); float _sa = _sin(angle_el); // Inverse park transform @@ -247,7 +315,7 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Function (iterative) generating open loop movement for target velocity // - target_velocity - rad/s // it uses voltage_limit variable -void StepperMotor::velocityOpenloop(float target_velocity){ +float StepperMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call @@ -260,17 +328,23 @@ void StepperMotor::velocityOpenloop(float target_velocity){ // for display purposes shaft_velocity = target_velocity; + // use voltage limit or current limit + float Uq = voltage_limit; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; + + return Uq; } // Function (iterative) generating open loop movement towards the target angle // - target_angle - rad // it uses voltage_limit and velocity_limit variables -void StepperMotor::angleOpenloop(float target_angle){ +float StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call @@ -287,9 +361,15 @@ void StepperMotor::angleOpenloop(float target_angle){ shaft_angle = target_angle; shaft_velocity = 0; } + + // use voltage limit or current limit + float Uq = voltage_limit; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; + + return Uq; } \ No newline at end of file diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 6993c68f..d0cd3e18 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -22,9 +22,10 @@ class StepperMotor: public FOCMotor public: /** StepperMotor class constructor - @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) + @param pp pole pair number + @param R motor phase resistance */ - StepperMotor(int pp); + StepperMotor(int pp, float R = NOT_SET); /** * Function linking a motor and a foc driver @@ -65,7 +66,7 @@ class StepperMotor: public FOCMotor */ void loopFOC() override; /** - * Function executing the control loops set by the controller parameter of the BLDCMotor. + * Function executing the control loops set by the controller parameter of the StepperMotor. * * @param target Either voltage, angle or velocity based on the motor.controller * If it is not set the motor will use the target set in its variable motor.target @@ -92,7 +93,7 @@ class StepperMotor: public FOCMotor /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ - void absoluteZeroSearch(); + int absoluteZeroSearch(); // Open loop motion control /** @@ -101,14 +102,14 @@ class StepperMotor: public FOCMotor * * @param target_velocity - rad/s */ - void velocityOpenloop(float target_velocity); + float velocityOpenloop(float target_velocity); /** * Function (iterative) generating open loop movement towards the target angle * it uses voltage_limit and velocity_limit variables * * @param target_angle - rad */ - void angleOpenloop(float target_angle); + float angleOpenloop(float target_angle); // open loop variables long open_loop_timestamp; }; diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index f3a2e342..5764aa30 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -32,7 +32,7 @@ class BLDCDriver{ virtual void setPwm(float Ua, float Ub, float Uc) = 0; /** - * Set phase voltages to the harware + * Set phase state, enable/disable * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 357063b0..c918b052 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -172,7 +172,7 @@ class FOCMotor PIDController P_angle{DEF_P_ANGLE_P,0,0,1e10,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 = 0; //!< parameter defining the ratio of downsampling for move commad + unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad // sensor related variabels @@ -193,7 +193,7 @@ class FOCMotor * significantly slowing the execution down!!!! */ void monitor(); - unsigned int monitor_downsample = 10; //!< show monitor outputs each monitor_downsample calls + unsigned int monitor_downsample = DEF_MON_DOWNSMAPLE; //!< show monitor outputs each monitor_downsample calls // initial monitoring will display target, voltage, velocity and angle uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor diff --git a/src/common/defaults.h b/src/common/defaults.h index 27c2cbcc..9c8cee3a 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -30,6 +30,10 @@ // default current limit values #define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default +// default monitor downsample +#define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample +#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable + // angle P params #define DEF_P_ANGLE_P 20.0 //!< default P controller P value #define DEF_VEL_LIM 20.0 //!< angle velocity limit default diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 1307b16e..7db81202 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -208,7 +208,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->torque_controller = (TorqueControlType)value; switch(motor->torque_controller){ case TorqueControlType::voltage: - println(F("dc volt")); + println(F("volt")); break; case TorqueControlType::dc_current: println(F("dc curr")); @@ -286,7 +286,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { println(motor->shaft_velocity); break; case 6: // get angle - printVerbose(F("Angle: ")); + printVerbose(F("angle: ")); println(motor->shaft_angle); break; default: @@ -303,11 +303,11 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->monitor_variables = (uint8_t) 0; println(F("clear")); break; - case SCMD_SET: - motor->monitor_variables = (uint8_t) 0; + 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; - motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); + if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); print( (user_command[value_index+i] - '0') ); } println(""); diff --git a/src/communication/commands.h b/src/communication/commands.h index 5504c87f..69405629 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -3,7 +3,7 @@ // see docs.simplefoc.com for in depth explanation of the commands -// list o commands +// list of commands #define CMD_C_D_PID 'D' //!< current d PID & LPF #define CMD_C_Q_PID 'Q' //!< current d PID & LPF #define CMD_V_PID 'V' //!< velocity PID & LPF @@ -16,11 +16,12 @@ #define CMD_MONITOR 'M' //!< monitoring #define CMD_RESIST 'R' //!< motor phase resistance + // commander configuration #define CMD_SCAN '?' //!< command scaning the network - only for commander #define CMD_VERBOSE '@' //!< command setting output mode - only for commander #define CMD_DECIMAL '#' //!< command setting decimal places - only for commander -// subcomands + // subcomands //pid - lpf #define SCMD_PID_P 'P' //!< PID gain P #define SCMD_PID_I 'I' //!< PID gain I diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index ed395325..d5d10881 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -32,15 +32,16 @@ void InlineCurrentSense::calibrateOffsets(){ offset_ia =0; offset_ib= 0; offset_ic= 0; - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 500; i++) { + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < 1000; 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 / 500.0; - offset_ib = offset_ib / 500.0; + offset_ia = offset_ia / 1000.0; + offset_ib = offset_ib / 1000.0; if(_isset(pinC)) offset_ic = offset_ic / 500.0; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 428e0ad1..d3678f00 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -48,7 +48,7 @@ int StepperDriver4PWM::init() { pinMode(pwm2A, OUTPUT); pinMode(pwm2B, OUTPUT); if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT); - if (_isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT); + if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT); // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; From a553db0f87807d2a6ee76f519125bc1cb15447c4 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Mar 2021 21:14:12 +0100 Subject: [PATCH 120/749] fix - typo in commander --- .github/workflows/ccpp.yml | 2 +- .../nano33IoT_velocity_control.ino | 2 +- ...32_fullcontrol.ino => simplefoc_osc_esp32_fullcontrol.ino} | 0 keywords.txt | 4 ++++ src/communication/Commander.cpp | 4 ++-- src/communication/Commander.h | 4 ++-- 6 files changed, 10 insertions(+), 6 deletions(-) rename examples/hardware_specific_examples/SAMD_examples/{magnetic_sensor => nano33IoT/nano33IoT_velocity_control}/nano33IoT_velocity_control.ino (95%) rename examples/osc_control_examples/osc_esp32_fullcontrol/{osc_esp32_fullcontrol.ino => simplefoc_osc_esp32_fullcontrol.ino} (100%) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 27a86e9e..2377cd61 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control diff --git a/examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino similarity index 95% rename from examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino rename to examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index a7fef55e..fc46a338 100644 --- a/examples/hardware_specific_examples/SAMD_examples/magnetic_sensor/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -20,7 +20,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); float target_velocity = 2.0; // instantiate the commander Commander command = Commander(SerialUSB); -void doTarget(char* cmd) { command.variable(&target_velocity, cmd); } +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino similarity index 100% rename from examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino rename to examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino diff --git a/keywords.txt b/keywords.txt index cfec8b76..c2e91bf2 100644 --- a/keywords.txt +++ b/keywords.txt @@ -88,6 +88,10 @@ attach KEYWORD2 enableInterrupt KEYWORD2 getValue KEYWORD2 handle KEYWORD2 +scalar KEYWORD2 +pid KEYWORD2 +lpf KEYWORD2 +motor KEYWORD2 diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index dd16642e..c26518c8 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -35,8 +35,8 @@ void Commander::run(){ } } -void Commander::run(HardwareSerial& serial){ - HardwareSerial* tmp = com_port; // save the serial instance +void Commander::run(Stream& serial){ + 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; diff --git a/src/communication/Commander.h b/src/communication/Commander.h index e74c5be6..0b07b707 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -61,9 +61,9 @@ class Commander * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes * - * @param reader - HardwareSerial to read user input + * @param reader - Stream to read user input */ - void run(HardwareSerial &reader); + void run(Stream &reader); /** * 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 From 83495ab04e9e9bf763c3da798a24d427ba33cbaf Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Mar 2021 21:23:15 +0100 Subject: [PATCH 121/749] fix typo in monitor + typo in example name --- ...efoc_osc_esp32_fullcontrol.ino => osc_esp32_fullcontrol.ino} | 0 src/common/base_classes/FOCMotor.cpp | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename examples/osc_control_examples/osc_esp32_fullcontrol/{simplefoc_osc_esp32_fullcontrol.ino => osc_esp32_fullcontrol.ino} (100%) diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino similarity index 100% rename from examples/osc_control_examples/osc_esp32_fullcontrol/simplefoc_osc_esp32_fullcontrol.ino rename to examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 99feaf46..eae18e67 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -93,9 +93,9 @@ void FOCMotor::monitor() { } if(monitor_variables & _MON_VOLT_Q) { monitor_port->print(voltage.q,4); + monitor_port->print("\t"); printed= true; } - monitor_port->print("\t"); if(monitor_variables & _MON_VOLT_D) { monitor_port->print(voltage.d,4); monitor_port->print("\t"); From d1fdfb7efed72126a5a05411d1735d593b7e9e97 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Mar 2021 21:40:26 +0100 Subject: [PATCH 122/749] added a bit better monitoring to shield examples --- .../single_full_control_example.ino | 2 ++ .../single_full_control_example.ino | 4 +++- keywords.txt | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index ca6a0998..5d02ecf2 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -53,6 +53,8 @@ void setup() { Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); + motor.monitor_downsample = 0; // disable intially + motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle // initialise motor motor.init(); 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 89bbade3..70444520 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 @@ -56,7 +56,9 @@ void setup() { Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + motor.monitor_downsample = 0; // disable intially + motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); diff --git a/keywords.txt b/keywords.txt index c2e91bf2..9ec297c8 100644 --- a/keywords.txt +++ b/keywords.txt @@ -151,7 +151,7 @@ voltage_power_supply KEYWORD2 voltage_sensor_align KEYWORD2 velocity_index_search KEYWORD2 monitor_downsample KEYWORD2 -monitor_downsample KEYWORD2 +monitor_variables KEYWORD2 motion_downsample KEYWORD2 pinA KEYWORD2 From b30b0f559b8ded566b2a77e7d6a345cbb83ce356 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 4 Mar 2021 22:44:52 +0100 Subject: [PATCH 123/749] commander typo --- 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 c26518c8..acc004ad 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -275,11 +275,11 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; case 3: // get current q printVerbose(F("Cq: ")); - println(motor->voltage.q); + println(motor->current.q); break; case 4: // get current d printVerbose(F("Cd: ")); - println(motor->voltage.q); + println(motor->current.q); break; case 5: // get velocity printVerbose(F("vel: ")); From 678c6f2bd0e33309476991539fbc0e080a680317 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 6 Mar 2021 00:46:29 +0100 Subject: [PATCH 124/749] 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 95783d0e2b924786bca06eb3f3e44ad8ac152ee2 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 6 Mar 2021 15:39:11 -0600 Subject: [PATCH 125/749] Commander err println --- src/communication/Commander.cpp | 34 ++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index acc004ad..6ee08d07 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -28,7 +28,7 @@ void Commander::run(){ // execute the user command run(received_chars); - // reset the command buffer + // reset the command buffer received_chars[0] = 0; rec_cnt=0; } @@ -36,9 +36,9 @@ void Commander::run(){ } void Commander::run(Stream& serial){ - Stream* tmp = com_port; // save the serial instance + 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; + if(!tmp) com_port = &serial; // a string to hold incoming data while (serial.available()) { @@ -49,7 +49,7 @@ void Commander::run(Stream& serial){ // execute the user command run(received_chars); - // reset the command buffer + // reset the command buffer received_chars[0] = 0; rec_cnt=0; } @@ -111,27 +111,27 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ - case CMD_C_Q_PID: // + case CMD_C_Q_PID: // printVerbose(F("PID curr q| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]); else pid(&motor->PID_current_q,&user_command[1]); break; - case CMD_C_D_PID: // + case CMD_C_D_PID: // printVerbose(F("PID curr d| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]); else pid(&motor->PID_current_d, &user_command[1]); break; - case CMD_V_PID: // + case CMD_V_PID: // printVerbose(F("PID vel| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]); else pid(&motor->PID_velocity, &user_command[1]); break; - case CMD_A_PID: // + case CMD_A_PID: // printVerbose(F("PID angle| ")); if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]); else pid(&motor->P_angle, &user_command[1]); break; - case CMD_LIMITS: // + case CMD_LIMITS: // printVerbose(F("Limits| ")); switch (sub_cmd){ case SCMD_LIM_VOLT: // voltage limit change @@ -294,20 +294,20 @@ void Commander::motor(FOCMotor* motor, char* user_command) { break; } break; - case SCMD_DOWNSAMPLE: + case SCMD_DOWNSAMPLE: printVerbose(F("downsample: ")); if(!GET) motor->monitor_downsample = value; println((int)motor->monitor_downsample); break; - case SCMD_CLEAR: - motor->monitor_variables = (uint8_t) 0; + case SCMD_CLEAR: + motor->monitor_variables = (uint8_t) 0; println(F("clear")); break; - case SCMD_SET: - if(!GET) motor->monitor_variables = (uint8_t) 0; + 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(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); + if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); print( (user_command[value_index+i] - '0') ); } println(""); @@ -371,7 +371,7 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){ printVerbose(F("Tf: ")); if(!GET) lpf->Tf = value; println(lpf->Tf); - break; + break; default: printError(); break; @@ -435,5 +435,5 @@ void Commander::printVerbose(const __FlashStringHelper *message){ if(verbose == VerboseMode::user_friendly) print(message); } void Commander::printError(){ - print(F("err")); + println(F("err")); } From 50beba6270bf8bbaba46acf1da133534fbd05db7 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 8 Mar 2021 01:05:33 -0600 Subject: [PATCH 126/749] Fix current limit + res and volt mode --- src/BLDCMotor.cpp | 80 ++++++++++++++++++++++++----------------------- 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 545427cd..76dc7032 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -44,6 +44,8 @@ void BLDCMotor::init() { PID_current_d.limit = voltage_limit; // velocity control loop controls current PID_velocity.limit = current_limit; + }else if(!current_sense && _isset(phase_resistance)){ + PID_velocity.limit = current_limit; }else{ PID_velocity.limit = voltage_limit; } @@ -101,32 +103,32 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. + // and checks the direction of measuremnt. _delay(500); - if(exit_flag){ + if(exit_flag){ if(current_sense) exit_flag *= alignCurrentSense(); else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); } - + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } - + return exit_flag; } // Calibarthe the motor and current sense phases int BLDCMotor::alignCurrentSense() { - int exit_flag = 1; // success + int exit_flag = 1; // success if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); // align current sense and the driver exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); - if(!exit_flag){ + if(!exit_flag){ // error in current sense - phase either not measured or bad connection if(monitor_port) monitor_port->println(F("MOT: Align error!")); exit_flag = 0; @@ -143,7 +145,7 @@ int BLDCMotor::alignCurrentSense() { int BLDCMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - + // if unknown natural direction if(!_isset(sensor_direction)){ // check if sensor needs zero search @@ -169,7 +171,7 @@ int BLDCMotor::alignSensor() { float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); - // determine the direction the sensor moved + // determine the direction the sensor moved if (mid_angle == end_angle) { if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); return 0; // failed calibration @@ -180,7 +182,7 @@ int BLDCMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); sensor_direction = Direction::CW; } - // check pole pair number + // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! @@ -193,7 +195,7 @@ int BLDCMotor::alignSensor() { // zero electric angle not known if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees + // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); @@ -212,7 +214,7 @@ int BLDCMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroSearch() { - + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -243,7 +245,7 @@ int BLDCMotor::absoluteZeroSearch() { // The faster it can be run the better void BLDCMotor::loopFOC() { // if disabled do nothing - if(!enabled) return; + if(!enabled) return; // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; @@ -263,7 +265,7 @@ void BLDCMotor::loopFOC() { // filter the value values current.q = LPF_current_q(current.q); // calculate the phase voltage - voltage.q = PID_current_q(current_sp - current.q); + voltage.q = PID_current_q(current_sp - current.q); voltage.d = 0; break; case TorqueControlType::foc_current: @@ -274,7 +276,7 @@ void BLDCMotor::loopFOC() { current.q = LPF_current_q(current.q); current.d = LPF_current_d(current.d); // calculate the phase voltages - voltage.q = PID_current_q(current_sp - current.q); + voltage.q = PID_current_q(current_sp - current.q); voltage.d = PID_current_d(-current.d); break; default: @@ -282,7 +284,7 @@ void BLDCMotor::loopFOC() { if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); break; } - + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -294,7 +296,7 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { // if disabled do nothing - if(!enabled) return; + if(!enabled) return; // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -307,8 +309,8 @@ void BLDCMotor::move(float new_target) { case MotionControlType::torque: if(torque_controller == TorqueControlType::voltage) // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; - else voltage.q = target*phase_resistance; - else + else voltage.q = target*phase_resistance; + else current_sp = target; // if current/foc_current torque control break; case MotionControlType::angle: @@ -318,7 +320,7 @@ void BLDCMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage + // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; @@ -331,7 +333,7 @@ void BLDCMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control + // if torque controlled through voltage control if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; @@ -353,7 +355,7 @@ void BLDCMotor::move(float new_target) { break; } } - + // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Space Vector PWM and Sine PWM algorithms @@ -376,9 +378,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { }; // static int trap_120_state = 0; sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + // centering the voltages around either + // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = modulation_centered ? (driver->voltage_limit)/2 : Uq; if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){ @@ -391,7 +393,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ub = center; Uc = trap_120_map[sector][2] * Uq + center; driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ + }else{ Ua = trap_120_map[sector][0] * Uq + center; Ub = trap_120_map[sector][1] * Uq + center; Uc = center; @@ -407,9 +409,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { }; // static int trap_150_state = 0; sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 + // centering the voltages around either + // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = modulation_centered ? (driver->voltage_limit)/2 : Uq; if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){ @@ -422,7 +424,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ub = center; Uc = trap_150_map[sector][2] * Uq + center; driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ + }else{ Ua = trap_150_map[sector][0] * Uq + center; Ub = trap_150_map[sector][1] * Uq + center; Uc = center; @@ -468,7 +470,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // 1) Ualpha, Ubeta // 2) Uout = sqrt(Ualpha^2 + Ubeta^2) // 3) angle_el = atan2(Ubeta, Ualpha) - // + // // equivalent to 2) because the magnitude does not change is: // Uout = sqrt(Ud^2 + Uq^2) // equivalent to 3) is @@ -476,7 +478,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { float Uout; // a bit of optitmisation - if(Ud){ // only if Ud and Uq set + if(Ud){ // only if Ud and Uq set // _sqrt is an approx of sqrt (3-4% error) Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; // angle normalisation in between 0 and 2pi @@ -562,16 +564,16 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to achieve target velocity - shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); + shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -591,7 +593,7 @@ float BLDCMotor::angleOpenloop(float target_angle){ // calculate the sample time from last call float Ts = (now_us - open_loop_timestamp) * 1e-6; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) @@ -603,15 +605,15 @@ float BLDCMotor::angleOpenloop(float target_angle){ shaft_velocity = 0; } - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; - + return Uq; } From 491a4f070d8d84d5b971aa07e85b5438da1a5c0a Mon Sep 17 00:00:00 2001 From: Armaan Roshani Date: Mon, 15 Mar 2021 04:02:53 +0300 Subject: [PATCH 127/749] Removed encoder init from mag sensor sketch Removed extraneous encoder initialization from magnetic sensor pole pair finder utility --- .../find_pole_pairs_number/find_pole_pairs_number.ino | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 c0711b64..4f6e83cc 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 @@ -24,12 +24,6 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperMotor motor = StepperMotor(1); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); -// Encoder(int encA, int encB , int cpr, int index) -Encoder encoder = Encoder(2, 3, 2048); -// interrupt routine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); // magnetic sensor instance - I2C @@ -173,4 +167,4 @@ void serialReceiveUserCommand() { received_chars = ""; } } -} \ No newline at end of file +} 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 128/749] 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 129/749] 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 130/749] 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 ee2dfdeee62bc28fdee5820fb1d26a0af4dc80c9 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 9 Apr 2021 18:29:01 +0200 Subject: [PATCH 131/749] powershield examples + hall_sensor examples rename --- .../single_full_control_example.ino | 98 +++++++++++++++++++ ...rs_example.ino => hall_sensor_example.ino} | 0 2 files changed, 98 insertions(+) create mode 100644 examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino rename examples/utils/sensor_test/hall_sensors/hall_sensor_example/{hall_sensors_example.ino => hall_sensor_example.ino} (100%) diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino new file mode 100644 index 00000000..16c7755b --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -0,0 +1,98 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8, 4, 7); + +// encoder instance +Encoder encoder = Encoder(10, 11, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// inline current sensor instance +InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); + +// commander communication instance +Commander command = Commander(Serial); +void doMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 20; + driver.init(); + // link driver + motor.linkDriver(&driver); + + motor.voltage_sensor_align = 1; + // set control loop type to be used + motor.torque_controller = TorqueControlType::foc_current; + motor.controller = MotionControlType::torque; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05; + motor.PID_velocity.I = 1; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 20; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_downsample = 0; // disable intially + motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle + + // current sense init and linking + current_sense.init(); + motor.linkCurrentSense(¤t_sense); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // subscribe motor to the commander + command.add('M', doMotor, "motor"); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // motor monitoring + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensors_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino similarity index 100% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensors_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino From eed0387916249bcab7c40eb7b78e8331f188b85f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 9 Apr 2021 18:32:10 +0200 Subject: [PATCH 132/749] 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 133/749] 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 134/749] 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 135/749] 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 136/749] 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 137/749] 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 138/749] 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 139/749] 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 140/749] 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 141/749] 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 142/749] 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 143/749] 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 144/749] 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 145/749] 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 146/749] 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 147/749] 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 148/749] 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 149/749] 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 150/749] 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 151/749] 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 152/749] 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 153/749] 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 a71f12ca77e3e53a6f9766138c0309b882ea335b Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 11:12:36 -0600 Subject: [PATCH 154/749] Samd support add support for serial insterface other than Serial I use -D SIMPLEFOC_SAMD_DEBUG_SERIAL=SERIAL_PORT_MONITOR --- src/drivers/hardware_specific/samd21_mcu.cpp | 34 ++-- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +-- src/drivers/hardware_specific/samd_mcu.cpp | 162 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 5 +- 4 files changed, 112 insertions(+), 109 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" From 5a9f0922ad621bf3ca9789d80a3962370ea4891f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 11 May 2021 00:36:45 +0200 Subject: [PATCH 155/749] 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 156/749] 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 157/749] 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 158/749] 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 159/749] 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 d37f34ebeb0d9fa75e39c5302144d1564ffd1fb1 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 10:18:17 +0200 Subject: [PATCH 160/749] initial low side added --- src/current_sense/InlineCurrentSense.cpp | 14 +- src/current_sense/LowsideCurrentSense.cpp | 179 ++++++++++++++++++ src/current_sense/LowsideCurrentSense.h | 59 ++++++ src/current_sense/hardware_api.h | 23 ++- .../hardware_specific/esp32_mcu.cpp | 80 ++++++++ .../hardware_specific/generic_mcu.cpp | 8 +- 6 files changed, 350 insertions(+), 13 deletions(-) create mode 100644 src/current_sense/LowsideCurrentSense.cpp create mode 100644 src/current_sense/LowsideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/esp32_mcu.cpp diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 2e9c4421..879395d1 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -22,7 +22,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // Inline sensor init function void InlineCurrentSense::init(){ // configure ADC variables - _configureADC(pinA,pinB,pinC); + _configureADCInline(pinA,pinB,pinC); // calibrate zero offsets calibrateOffsets(); } @@ -36,9 +36,9 @@ void InlineCurrentSense::calibrateOffsets(){ offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltage(pinA); - offset_ib += _readADCVoltage(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + offset_ia += _readADCVoltageInline(pinA); + offset_ib += _readADCVoltageInline(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); _delay(1); } // calculate the mean offsets @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp new file mode 100644 index 00000000..abebf03d --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -0,0 +1,179 @@ +#include "LowsideCurrentSense.h" +// LowsideCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Lowside sensor init function +void LowsideCurrentSense::init(){ + // configure ADC variables + _configureADCLowSide(pinA,pinB,pinC); + // calibrate zero offsets + calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowsideCurrentSense::calibrateOffsets(){ + // find adc offset = zero current voltage + offset_ia =0; + offset_ib= 0; + offset_ic= 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < 1000; i++) { + offset_ia += _readADCVoltageLowSide(pinA); + offset_ib += _readADCVoltageLowSide(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / 1000.0; + offset_ib = offset_ib / 1000.0; + if(_isset(pinC)) offset_ic = offset_ic / 1000.0; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowsideCurrentSense::driverSync(BLDCDriver *driver){ + _driverSyncLowSide(); + return 1; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + //gain_a *= -1; + //gain_b *= -1; + //gain_c *= -1; + + /* + int exit_flag = 1; + if(skip_align) return exit_flag; + + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = fabs(c.a / c.b); + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if( ab_ratio > 1.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = fabs(c.b/c.a); + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if( ba_ratio > 1.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // if phase C measured + if(_isset(pinC)){ + // set phase B active and phases A and C down + driver->setPwm(0, 0, voltage); + _delay(200); + c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 50; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.c = (c.c+c1.c)/50.0; + } + driver->setPwm(0, 0, 0); + gain_c *= _sign(c.c); + } + + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + + return exit_flag; + */ + return 1; +} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h new file mode 100644 index 00000000..2300d5f6 --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.h @@ -0,0 +1,59 @@ +#ifndef LOWSIDE_CS_LIB_H +#define LOWSIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/FOCMotor.h" +#include "hardware_api.h" + + +class LowsideCurrentSense: public CurrentSense{ + public: + /** + LowsideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ab376e8e..4c4c1086 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -9,7 +9,7 @@ * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltage(const int pinA); +float _readADCVoltageInline(const int pinA); /** * function reading an ADC value and returning the read voltage @@ -18,6 +18,25 @@ float _readADCVoltage(const int pinA); * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); +void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA); + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide(); #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..7f84f9a9 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,80 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 4095.0 + +static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; +int a1, a2, a3; //Current readings from internal current sensor amplifiers +int _pinA, _pinB, _pinC; +static void IRAM_ATTR isr_handler(void*); +byte currentState = 1; + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin){ + uint32_t raw_adc; + + if (pin == _pinA) raw_adc = a1; + else if (pin == _pinB) raw_adc = a2; + else if (pin == _pinC) raw_adc = a3; + + return raw_adc * _ADC_CONV; +} + + +// function reading an ADC value and returning the read voltage +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + +void _driverSyncLowSide(){ + MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt + mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +// Read currents when interrupt is triggered +static void IRAM_ATTR isr_handler(void*){ + uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; + uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; + //uint32_t mcpwm_intr_status_2 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + + if(mcpwm_intr_status_0 > 0 && currentState == 1){ + a1 = analogRead(_pinA); + //a2 = analogRead(_pinB); + currentState = 2; + } + else if(mcpwm_intr_status_1 > 0 && currentState == 2){ + a2 = analogRead(_pinB); + //a3 = analogRead(_pinC); + currentState = 1; + } + /* + else if(mcpwm_intr_status_2 > 0 && currentState == 3){ + a3 = analogRead(_pinC); + //a1 = analogRead(_pinA); + currentState = 1; + }*/ + + MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; + MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; + //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; +} + +#endif diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 423774c2..dccd3601 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -18,8 +18,8 @@ #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 #elif defined(ESP_H) // or esp32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 4095.0 + // do nothing implemented in esp32_mcu.h + #elif defined(_STM32_DEF_) // or stm32 #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 1024.0 @@ -33,14 +33,14 @@ #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) // function reading an ADC value and returning the read voltage -float _readADCVoltage(const int pinA){ +float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); return raw_adc * _ADC_CONV; } // function reading an ADC value and returning the read voltage -void _configureADC(const int pinA,const int pinB,const int pinC){ +void _configureADCInline(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); From 9220eaddaed7a3544fd8c3bea4d678e7715eb2d6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 13 May 2021 11:16:22 +0200 Subject: [PATCH 161/749] 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 162/749] 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 163/749] 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 164/749] 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 165/749] 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 166/749] 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 167/749] 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 168/749] 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 169/749] 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 170/749] 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 171/749] 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 172/749] 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 173/749] 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 174/749] 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); From c21dc2e22df6b4e9d8fffbbf9df8b6e25fd1d234 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 19:19:06 -0600 Subject: [PATCH 175/749] LowSideCurrentSense: first (truly) successful DMA+ADC readings, 1.65-centered readings with the drv8305, just as intented --- src/current_sense/LowSideCurrentSense.cpp | 177 ++++++++++++ src/current_sense/LowSideCurrentSense.h | 57 ++++ src/current_sense/hardware_api.h | 4 + .../hardware_specific/samd21_mcu.cpp | 266 ++++++++++++++++++ 4 files changed, 504 insertions(+) create mode 100644 src/current_sense/LowSideCurrentSense.cpp create mode 100644 src/current_sense/LowSideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp new file mode 100644 index 00000000..f9647f7b --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -0,0 +1,177 @@ +#include "LowSideCurrentSense.h" +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Inline sensor init function +void LowSideCurrentSense::init(){ + // configure ADC variables + _configure3PinsDMA(pinA,pinB,pinC); + _start3PinsDMA(); //start next acuisition + // calibrate zero offsets + // calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowSideCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + offset_ia += _readADCVoltage(pinA); + offset_ib += _readADCVoltage(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps + // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps + // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + _start3PinsDMA(); + + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowSideCurrentSense::driverSync(BLDCDriver *driver){ + return 1; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int LowSideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + int exit_flag = 1; + if(skip_align) return exit_flag; + + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = fabs(c.a / c.b); + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if( ab_ratio > 1.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = fabs(c.b/c.a); + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if( ba_ratio > 1.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // if phase C measured + if(_isset(pinC)){ + // set phase B active and phases A and C down + driver->setPwm(0, 0, voltage); + _delay(200); + c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 50; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.c = (c.c+c1.c)/50.0; + } + driver->setPwm(0, 0, 0); + gain_c *= _sign(c.c); + } + + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + return exit_flag; +} + diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h new file mode 100644 index 00000000..48c6dc80 --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.h @@ -0,0 +1,57 @@ +#ifndef LOW_SIDE_CS_LIB_H +#define LOW_SIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "hardware_api.h" + +class LowSideCurrentSense: public CurrentSense{ + public: + /** + LowSideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ab376e8e..83377820 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,4 +20,8 @@ float _readADCVoltage(const int pinA); */ void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); +void _start3PinsDMA(); +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC = NOT_SET); + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..fb0dbeaf --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,266 @@ + +#include "../hardware_api.h" + +// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless +static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout +static uint32_t ADC_FirstPin; // PA04 +static uint32_t ADC_LastPin; // PA06 +static uint32_t BufferSize = 0; + +uint16_t adcBuffer[20]; + + +static uint32_t ADC_pinA = A4; +static uint32_t ADC_pinB = A5; +static uint32_t ADC_pinC = 8; + + +#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val +#define _ADC_RESOLUTION (1 << 12) +#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) + +void adc_dma(void *rxdata, size_t hwords); +void adc_init(); +void adc_start_with_DMA(); +void dma_init(); + +void _start3PinsDMA() +{ + adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); + adc_start_with_DMA(); +} +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +{ + while(ADC->CTRLA.bit.ENABLE) ; + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[adcA] * ADC_CONV_; + b = adcBuffer[adcB] * ADC_CONV_; + if(_isset(pinC)) + { + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[adcC] * ADC_CONV_; + } +} + +// function reading an ADC value and returning the read voltage +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ + + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + + pinMode(42, INPUT); + ADC_FirstPin = min(adcA, adcB); + ADC_LastPin = max(adcA, adcB); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) + { + pinMode(pinC, INPUT); + ADC_FirstPin = min(ADC_FirstPin, adcC); + ADC_LastPin = max(ADC_LastPin, adcC); + } + + ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout + BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + + ADC_pinA = pinA; + ADC_pinB = pinB; + ADC_pinC = pinC; + + // ADC and DMA + adc_init(); + dma_init(); +} + + + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) + +static uint32_t ADC_DMA_chnl = 3; // DMA channel + + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +/** + * @brief Initialize ADC + * @retval void + */ +void adc_init(){ + + auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X + // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + ADCsync(); // ref 31.6.16 + + a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + SerialUSB.println("--------------"); + SerialUSB.println(a); + SerialUSB.println(b); + SerialUSB.println(c); + SerialUSB.println("--------------"); +/* +Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan +This register gives the number of input sources included in the pin scan. The number of input sources included is +INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + +INPUTOFFSET + INPUTSCAN. +The range of the scan mode must not exceed the number of input channels available on the device. +Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection +These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If +the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit +group in the SamplingControl register must be written. +Table 32-14. Positive Mux Input Selection +MUXPOS[4:0] Group configuration Description +0x00 PIN0 ADC AIN0 pin +0x01 PIN1 ADC AIN1 pin +0x02 PIN2 ADC AIN2 pin +0x03 PIN3 ADC AIN3 pin +0x04 PIN4 ADC AIN4 pin +0x05 PIN5 ADC AIN5 pin +0x06 PIN6 ADC AIN6 pin +0x07 PIN7 ADC AIN7 pin +0x08 PIN8 ADC AIN8 pin +0x09 PIN9 ADC AIN9 pin +0x0A PIN10 ADC AIN10 pin +0x0B PIN11 ADC AIN11 pin +0x0C PIN12 ADC AIN12 pin +0x0D PIN13 ADC AIN13 pin +0x0E PIN14 ADC AIN14 pin +0x0F PIN15 ADC AIN15 pin +0x10 PIN16 ADC AIN16 pin +0x11 PIN17 ADC AIN17 pin +0x12 PIN18 ADC AIN18 pin +0x13 PIN19 ADC AIN19 pin +0x14-0x17 Reserved +0x18 TEMP Temperature reference +0x19 BANDGAP Bandgap voltage +0x1A SCALEDCOREVCC 1/4 scaled core supply +0x1B SCALEDIOVCC 1/4 scaled I/O supply +0x1C DAC DAC output +0x1D-0x1F Reserved +*/ + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + + +/** + * @brief dma_init + * @retval void + */ +void dma_init() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + +/** + * @brief adc_dma + * @retval void + */ +void adc_dma(void *rxdata, size_t hwords) { + uint32_t temp_CHCTRLB_reg; + + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); + temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | +DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + +/** + * @brief adc_stop_with_DMA + * @retval void + */ +void adc_stop_with_DMA(void) +{ + ADC->CTRLA.bit.ENABLE = 0x00; + // SerialUSB.println("DMA stopped!"); +} + +/** + * @brief adc_start_with_DMA + * @retval void + */ +void adc_start_with_DMA(void) +{ + // SerialUSB.println("strating DMA..."); + + + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADC->SWTRIG.bit.FLUSH = 1; + ADC->CTRLA.bit.ENABLE = 0x01; +} + + +/** + * @brief DMAC_Handler + * @retval void + */ +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adc_stop_with_DMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; +} From e6d77b08e983ae4e3bfb2640a558f29d153dbf81 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 20:26:26 -0600 Subject: [PATCH 176/749] wrapped samd21 adc/dma implementation in an ugly class --- src/SimpleFOC.h | 1 + src/current_sense/LowSideCurrentSense.cpp | 13 +- src/current_sense/LowSideCurrentSense.h | 3 + .../hardware_specific/samd21_mcu.cpp | 208 ++++++++---------- .../hardware_specific/samd21_mcu.h | 60 +++++ 5 files changed, 168 insertions(+), 117 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index f9647f7b..0c12e528 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -5,7 +5,9 @@ // - phA - A phase adc pin // - phB - B phase adc pin // - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): +adc(_pinA, _pinB, _pinC) +{ pinA = _pinA; pinB = _pinB; pinC = _pinC; @@ -21,9 +23,7 @@ LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int // Inline sensor init function void LowSideCurrentSense::init(){ - // configure ADC variables - _configure3PinsDMA(pinA,pinB,pinC); - _start3PinsDMA(); //start next acuisition + adc.init(); // calibrate zero offsets // calibrateOffsets(); } @@ -54,8 +54,9 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - _start3PinsDMA(); + + adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + adc._start3PinsDMA(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 48c6dc80..2a898bfc 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -6,6 +6,7 @@ #include "../common/time_utils.h" #include "../common/base_classes/CurrentSense.h" #include "hardware_api.h" +#include "hardware_specific/samd21_mcu.h" class LowSideCurrentSense: public CurrentSense{ public: @@ -52,6 +53,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) + SAMDCurrentSensceADC adc; + }; #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index fb0dbeaf..c95bb754 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,35 +1,57 @@ -#include "../hardware_api.h" +#include "samd21_mcu.h" -// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout -static uint32_t ADC_FirstPin; // PA04 -static uint32_t ADC_LastPin; // PA06 -static uint32_t BufferSize = 0; -uint16_t adcBuffer[20]; +void adc_stop_with_DMA(void); +void adc_start_with_DMA(void); +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -static uint32_t ADC_pinA = A4; -static uint32_t ADC_pinB = A5; -static uint32_t ADC_pinC = 8; -#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val -#define _ADC_RESOLUTION (1 << 12) -#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) +SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) +: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) +{ + ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; +} + +void SAMDCurrentSensceADC::init() +{ + _configure3PinsDMA(); + _start3PinsDMA(); //s +} -void adc_dma(void *rxdata, size_t hwords); -void adc_init(); -void adc_start_with_DMA(); -void dma_init(); -void _start3PinsDMA() +void SAMDCurrentSensceADC::_start3PinsDMA() { adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); adc_start_with_DMA(); } -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) { while(ADC->CTRLA.bit.ENABLE) ; uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; @@ -44,7 +66,7 @@ void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, floa } // function reading an ADC value and returning the read voltage -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ +void SAMDCurrentSensceADC::_configure3PinsDMA(){ uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; @@ -65,10 +87,6 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; - ADC_pinA = pinA; - ADC_pinB = pinB; - ADC_pinC = pinC; - // ADC and DMA adc_init(); dma_init(); @@ -76,41 +94,17 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ -// ADC DMA sequential free running (6) with Interrupts ///////////////// - -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) - -static uint32_t ADC_DMA_chnl = 3; // DMA channel -/** - * @brief ADC sync wait - * @retval void - */ -static __inline__ void ADCsync() __attribute__((always_inline, unused)); -static void ADCsync() { - while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free -} - /** * @brief Initialize ADC * @retval void */ -void adc_init(){ +void SAMDCurrentSensceADC::adc_init(){ - auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); @@ -121,54 +115,46 @@ void adc_init(){ // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 - a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - c = analogRead(ADC_pinC); // do some pin init pinPeripheral() - SerialUSB.println("--------------"); - SerialUSB.println(a); - SerialUSB.println(b); - SerialUSB.println(c); - SerialUSB.println("--------------"); -/* -Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan -This register gives the number of input sources included in the pin scan. The number of input sources included is -INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + -INPUTOFFSET + INPUTSCAN. -The range of the scan mode must not exceed the number of input channels available on the device. -Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection -These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If -the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit -group in the SamplingControl register must be written. -Table 32-14. Positive Mux Input Selection -MUXPOS[4:0] Group configuration Description -0x00 PIN0 ADC AIN0 pin -0x01 PIN1 ADC AIN1 pin -0x02 PIN2 ADC AIN2 pin -0x03 PIN3 ADC AIN3 pin -0x04 PIN4 ADC AIN4 pin -0x05 PIN5 ADC AIN5 pin -0x06 PIN6 ADC AIN6 pin -0x07 PIN7 ADC AIN7 pin -0x08 PIN8 ADC AIN8 pin -0x09 PIN9 ADC AIN9 pin -0x0A PIN10 ADC AIN10 pin -0x0B PIN11 ADC AIN11 pin -0x0C PIN12 ADC AIN12 pin -0x0D PIN13 ADC AIN13 pin -0x0E PIN14 ADC AIN14 pin -0x0F PIN15 ADC AIN15 pin -0x10 PIN16 ADC AIN16 pin -0x11 PIN17 ADC AIN17 pin -0x12 PIN18 ADC AIN18 pin -0x13 PIN19 ADC AIN19 pin -0x14-0x17 Reserved -0x18 TEMP Temperature reference -0x19 BANDGAP Bandgap voltage -0x1A SCALEDCOREVCC 1/4 scaled core supply -0x1B SCALEDIOVCC 1/4 scaled I/O supply -0x1C DAC DAC output -0x1D-0x1F Reserved -*/ + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; ADCsync(); ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] @@ -176,7 +162,8 @@ MUXPOS[4:0] Group configuration Description ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; ADCsync(); @@ -187,7 +174,7 @@ MUXPOS[4:0] Group configuration Description * @brief dma_init * @retval void */ -void dma_init() { +void SAMDCurrentSensceADC::dma_init() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -201,7 +188,7 @@ void dma_init() { * @brief adc_dma * @retval void */ -void adc_dma(void *rxdata, size_t hwords) { +void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { uint32_t temp_CHCTRLB_reg; DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); @@ -209,7 +196,7 @@ void adc_dma(void *rxdata, size_t hwords) { DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | -DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -224,6 +211,9 @@ DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } + + + /** * @brief adc_stop_with_DMA * @retval void @@ -241,16 +231,12 @@ void adc_stop_with_DMA(void) void adc_start_with_DMA(void) { // SerialUSB.println("strating DMA..."); - - - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } - - /** * @brief DMAC_Handler * @retval void @@ -263,4 +249,4 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..fb5d319a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,60 @@ + +#include "../hardware_api.h" + + +class SAMDCurrentSensceADC +{ + +public: + + SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + + void init(); + // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout + uint32_t ADC_FirstPin; // PA04 + uint32_t ADC_LastPin; // PA06 + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA = A4; + uint32_t pinB = A5; + uint32_t pinC = 8; + + float _ADC_VOLTAGE; + float _ADC_RESOLUTION; + float ADC_CONV_; + + void _start3PinsDMA(); + void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); + // function reading an ADC value and returning the read voltage + void _configure3PinsDMA(); + + + + + uint32_t ADC_DMA_chnl = 3; // DMA channel + + + /** + * @brief Initialize ADC + * @retval void + */ + void adc_init(); + + + /** + * @brief dma_init + * @retval void + */ + void dma_init(); + + /** + * @brief adc_dma + * @retval void + */ + void adc_dma(void *rxdata, size_t hwords); + +}; From 0b7839f1e94356d4dde5757ddef412cce52d8331 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 May 2021 13:05:37 +0200 Subject: [PATCH 177/749] FEAT added weak definitions of generic hardware implementations + arduino due same frequency PWM and TC calculation --- src/common/foc_utils.h | 1 + .../hardware_specific/atmega328_mcu.cpp | 7 +- src/drivers/hardware_specific/due_mcu.cpp | 87 +++++++++++-------- src/drivers/hardware_specific/generic_mcu.cpp | 70 ++++++++------- src/drivers/hardware_specific/teensy_mcu.cpp | 14 --- 5 files changed, 95 insertions(+), 84 deletions(-) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 06978424..30e99b4f 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -9,6 +9,7 @@ #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) +#define _UNUSED(v) (void) (v) // utility defines #define _2_SQRT3 1.15470053838 diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index cf12c302..be8dadb1 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -6,7 +6,7 @@ void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { + if (pin == 5 || pin == 6 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } @@ -17,12 +17,12 @@ void _pinHighFrequency(const int pin){ } - // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -34,6 +34,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - hardware speciffic // supports Arudino/ATmega328 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -118,6 +119,8 @@ int _configureComplementaryPair(int pinH, int pinL) { // - hardware specific // supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); // High PWM frequency // - always max 32kHz int ret_flag = 0; diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 27097301..e0d39afd 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -1,6 +1,11 @@ #include "../hardware_api.h" -#if defined(__arm__) && defined(__SAM3X8E__) +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +#define _PWM_RES_MIN 255 // 50khz // pwm frequency and max duty cycle static unsigned long _pwm_frequency; @@ -101,7 +106,29 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if (!PWMEnabled) { // PWM Startup code pmc_enable_periph_clk(PWM_INTERFACE_ID); - PWMC_ConfigureClocks(pwm_freq * _max_pwm_value, 0, VARIANT_MCK); + // this function does not work too well - I'll rewrite it + // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK); + + // finding the divisors an prescalers form FindClockConfiguration function + uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024}; + uint8_t divisor = 0; + uint32_t prescaler; + + /* Find prescaler and divisor values */ + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + while ((prescaler > 255) && (divisor < 11)) { + divisor++; + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + } + // update the divisor*prescaler value + prescaler = prescaler | (divisor << 8); + + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler)) + _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler); + // set the prescaler value + PWM->PWM_CLK = prescaler; + PWMEnabled = 1; } @@ -112,6 +139,8 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ g_APinDescription[ulPin].ulPinType, g_APinDescription[ulPin].ulPin, g_APinDescription[ulPin].ulPinConfiguration); + // PWM_CMR_CALG - center align + // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0); PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0); PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value); PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0); @@ -123,7 +152,7 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin // We use MCLK/2 as clock. - const uint32_t TC = VARIANT_MCK / 2 / pwm_freq; + const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ; // Setup Timer for this pin ETCChannel channel = g_APinDescription[ulPin].ulTCChannel; uint32_t chNo = channelToChNo[channel]; @@ -131,17 +160,18 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ Tc *chTC = channelToTC[channel]; uint32_t interfaceID = channelToId[channel]; - if (!TCChanEnabled[interfaceID]) { - pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); - TC_Configure(chTC, chNo, - TC_CMR_TCCLKS_TIMER_CLOCK1 | - TC_CMR_WAVE | // Waveform mode - TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC - TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) - TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | - TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); - TC_SetRC(chTC, chNo, TC); - } + if (!TCChanEnabled[interfaceID]) { + pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); + TC_Configure(chTC, chNo, + TC_CMR_TCCLKS_TIMER_CLOCK1 | + TC_CMR_WAVE | // Waveform mode + TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC + TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) + TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | + TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); + TC_SetRC(chTC, chNo, TC); + } + // disable the counter on start if (chA){ TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET); @@ -197,7 +227,7 @@ void setPwm(uint32_t ulPin, uint32_t ulValue) { const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency; // Map value to Timer ranges 0..max_duty_cycle => 0..TC // Setup Timer for this pin - ulValue = ulValue * TC; + ulValue = ulValue * TC ; pwm_counter_vals[channel] = ulValue / _max_pwm_value; // enable counter if (channelToAB[channel]) @@ -297,8 +327,8 @@ void TC8_Handler() // - BLDC motor - 3PWM setting // - hardware specific void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -314,8 +344,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int //- Stepper driver - 2PWM setting // - hardware specific void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -329,8 +359,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // - Stepper motor - 4PWM setting // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -373,19 +403,4 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ } -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - return -1; -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - return; -} - - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 4a2360d9..6549de07 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,32 +1,13 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328 - -#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 - -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - -#elif defined(ESP_H) // or esp32 - -#elif defined(_STM32_DEF_) // or stm32 - -#elif defined(_SAMD21_) // samd21 - -#elif defined(_SAMD51_) // samd51 - -#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 - -#elif defined(TARGET_RP2040) - -#else - // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); + _UNUSED(pinA); + _UNUSED(pinB); return; } @@ -34,7 +15,11 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + _UNUSED(pwm_frequency); + _UNUSED(pinA); + _UNUSED(pinB); + _UNUSED(pinC); return; } @@ -43,14 +28,27 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + _UNUSED(pwm_frequency); + _UNUSED(pin1A); + _UNUSED(pin1B); + _UNUSED(pin2A); + _UNUSED(pin2B); return; } // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +__attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + _UNUSED(pinA_h); + _UNUSED(pinB_h); + _UNUSED(pinC_h); + _UNUSED(pinA_l); + _UNUSED(pinB_l); + _UNUSED(pinC_l); return -1; } @@ -58,7 +56,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] analogWrite(pinA, 255.0*dc_a); analogWrite(pinB, 255.0*dc_b); @@ -67,7 +65,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] analogWrite(pinA, 255.0*dc_a); analogWrite(pinB, 255.0*dc_b); @@ -77,7 +75,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] analogWrite(pin1A, 255.0*dc_1a); analogWrite(pin1B, 255.0*dc_1b); @@ -89,9 +87,17 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(dead_zone); + _UNUSED(pinA_h); + _UNUSED(pinB_h); + _UNUSED(pinC_h); + _UNUSED(pinA_l); + _UNUSED(pinB_l); + _UNUSED(pinC_l); return; } - -#endif diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 8ec19643..5f70db4f 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -74,18 +74,4 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in analogWrite(pin2B, 255.0*dc_2b); } - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - return -1; -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - return; -} #endif \ No newline at end of file From 9af38faec14e1ae8e0bbd46243a03f7ada6e6cc3 Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 15 May 2021 11:13:58 -0600 Subject: [PATCH 178/749] SAMDCurrentSenseADCDMA class --- src/current_sense/LowSideCurrentSense.cpp | 4 +- src/current_sense/LowSideCurrentSense.h | 2 +- .../hardware_specific/samd21_mcu.cpp | 157 ++++++------------ .../hardware_specific/samd21_mcu.h | 76 ++++----- 4 files changed, 91 insertions(+), 148 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 0c12e528..7f6b3ceb 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,8 +55,8 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - adc._start3PinsDMA(); + adc.readResults(current.a, current.b, current.c); + adc.startADCScan(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 2a898bfc..379e441c 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -53,7 +53,7 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - SAMDCurrentSensceADC adc; + SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index c95bb754..f511ec1d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,9 +1,10 @@ #include "samd21_mcu.h" + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -void adc_stop_with_DMA(void); -void adc_start_with_DMA(void); +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); /** * @brief ADC sync wait @@ -16,91 +17,66 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) -: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) { - ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); - this->pinA = pinA; - this->pinB = pinB; - this->pinC = pinC; + countsToVolts = ( voltageAREF / maxCountsADC ); } -void SAMDCurrentSensceADC::init() -{ - _configure3PinsDMA(); - _start3PinsDMA(); //s +void SAMDCurrentSenseADCDMA::init(){ + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() } -void SAMDCurrentSensceADC::_start3PinsDMA() -{ - adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); - adc_start_with_DMA(); +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); } -void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) -{ + +void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ while(ADC->CTRLA.bit.ENABLE) ; - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[adcA] * ADC_CONV_; - b = adcBuffer[adcB] * ADC_CONV_; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + a = adcBuffer[ainA] * countsToVolts; + b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[adcC] * ADC_CONV_; + c = adcBuffer[ainC] * countsToVolts; } } -// function reading an ADC value and returning the read voltage -void SAMDCurrentSensceADC::_configure3PinsDMA(){ +void SAMDCurrentSenseADCDMA::initPins(){ - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - - pinMode(42, INPUT); - ADC_FirstPin = min(adcA, adcB); - ADC_LastPin = max(adcA, adcB); + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); if( _isset(pinC) ) { pinMode(pinC, INPUT); - ADC_FirstPin = min(ADC_FirstPin, adcC); - ADC_LastPin = max(ADC_LastPin, adcC); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); } - ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout - BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; - // ADC and DMA - adc_init(); - dma_init(); } - - - - -/** - * @brief Initialize ADC - * @retval void - */ -void SAMDCurrentSensceADC::adc_init(){ +void SAMDCurrentSenseADCDMA::initADC(){ analogRead(pinA); // do some pin init pinPeripheral() analogRead(pinB); // do some pin init pinPeripheral() @@ -155,9 +131,9 @@ void SAMDCurrentSensceADC::adc_init(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -169,12 +145,9 @@ void SAMDCurrentSensceADC::adc_init(){ ADCsync(); } +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -/** - * @brief dma_init - * @retval void - */ -void SAMDCurrentSensceADC::dma_init() { +void SAMDCurrentSenseADCDMA::initDMA() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -184,68 +157,46 @@ void SAMDCurrentSensceADC::dma_init() { DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); } -/** - * @brief adc_dma - * @retval void - */ -void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { - uint32_t temp_CHCTRLB_reg; - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); - temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | - DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; descriptor.btcnt = hwords; descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); // start channel - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } - - -/** - * @brief adc_stop_with_DMA - * @retval void - */ -void adc_stop_with_DMA(void) -{ +void adcStopWithDMA(void){ ADC->CTRLA.bit.ENABLE = 0x00; - // SerialUSB.println("DMA stopped!"); } -/** - * @brief adc_start_with_DMA - * @retval void - */ -void adc_start_with_DMA(void) -{ - // SerialUSB.println("strating DMA..."); - // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; +void adcStartWithDMA(void){ ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } -/** - * @brief DMAC_Handler - * @retval void - */ + void DMAC_Handler() { uint8_t active_channel; active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adc_stop_with_DMA(); + adcStopWithDMA(); DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index fb5d319a..c410b522 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,60 +1,52 @@ #include "../hardware_api.h" - -class SAMDCurrentSensceADC + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + +class SAMDCurrentSenseADCDMA { public: - SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void init(); - // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless - uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout - uint32_t ADC_FirstPin; // PA04 - uint32_t ADC_LastPin; // PA06 - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA = A4; - uint32_t pinB = A5; - uint32_t pinC = 8; - - float _ADC_VOLTAGE; - float _ADC_RESOLUTION; - float ADC_CONV_; + void startADCScan(); + void readResults(float & a, float & b, float & c); - void _start3PinsDMA(); - void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); - // function reading an ADC value and returning the read voltage - void _configure3PinsDMA(); +private: + void adcToDMATransfer(void *rxdata, size_t hwords); + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; - - uint32_t ADC_DMA_chnl = 3; // DMA channel - - - /** - * @brief Initialize ADC - * @retval void - */ - void adc_init(); + uint16_t adcBuffer[20]; - /** - * @brief dma_init - * @retval void - */ - void dma_init(); + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel - /** - * @brief adc_dma - * @retval void - */ - void adc_dma(void *rxdata, size_t hwords); + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); }; From 7698c0ebcbd67d545c677d5db0087cfd4d1fc119 Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 08:32:25 -0600 Subject: [PATCH 179/749] FEAT ADC-DMA fix deadlock --- src/current_sense/LowSideCurrentSense.cpp | 7 +++++-- src/current_sense/LowSideCurrentSense.h | 3 ++- .../hardware_specific/samd21_mcu.cpp | 18 +++++++++++++----- .../hardware_specific/samd21_mcu.h | 6 +++--- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 7f6b3ceb..46110c34 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,10 +55,13 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc.readResults(current.a, current.b, current.c); + if(adc.readResults(current.a, current.b, current.c)) + { + oldCurrent = current; + } adc.startADCScan(); - return current; + return oldCurrent; } // Function synchronizing current sense with motor driver. // for in-line sensig no such thing is necessary diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 379e441c..461f3e35 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -52,7 +52,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ia; //!< zero current A voltage value (center of the adc reading) double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - + + PhaseCurrent_s oldCurrent; SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f511ec1d..e9f43ef7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -40,17 +40,19 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ - while(ADC->CTRLA.bit.ENABLE) ; +bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; a = adcBuffer[ainA] * countsToVolts; b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; c = adcBuffer[ainC] * countsToVolts; } + return true; } void SAMDCurrentSenseADCDMA::initPins(){ @@ -61,11 +63,11 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); @@ -158,7 +160,7 @@ void SAMDCurrentSenseADCDMA::initDMA() { } -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; @@ -183,13 +185,19 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { void adcStopWithDMA(void){ + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; + } void adcStartWithDMA(void){ + ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); } void DMAC_Handler() { diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c410b522..1c1603b9 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,6 +1,6 @@ #include "../hardware_api.h" - +#include typedef struct { uint16_t btctrl; uint16_t btcnt; @@ -18,11 +18,11 @@ class SAMDCurrentSenseADCDMA void init(); void startADCScan(); - void readResults(float & a, float & b, float & c); + bool readResults(float & a, float & b, float & c); private: - void adcToDMATransfer(void *rxdata, size_t hwords); + void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); From a0d17a097aadf6696697ef7428e59cc5dd2aaf97 Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 10 May 2021 11:12:36 -0600 Subject: [PATCH 180/749] Samd support add support for serial insterface other than Serial I use -D SIMPLEFOC_SAMD_DEBUG_SERIAL=SERIAL_PORT_MONITOR --- src/drivers/hardware_specific/samd21_mcu.cpp | 34 ++-- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +-- src/drivers/hardware_specific/samd_mcu.cpp | 162 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 5 +- 4 files changed, 112 insertions(+), 109 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" From db7d8653f498b54bd223034676d2c8a388833d62 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 19:19:06 -0600 Subject: [PATCH 181/749] LowSideCurrentSense: first (truly) successful DMA+ADC readings, 1.65-centered readings with the drv8305, just as intented --- src/current_sense/LowSideCurrentSense.cpp | 177 ++++++++++++ src/current_sense/LowSideCurrentSense.h | 57 ++++ src/current_sense/hardware_api.h | 19 -- .../hardware_specific/samd21_mcu.cpp | 266 ++++++++++++++++++ 4 files changed, 500 insertions(+), 19 deletions(-) create mode 100644 src/current_sense/LowSideCurrentSense.cpp create mode 100644 src/current_sense/LowSideCurrentSense.h create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp new file mode 100644 index 00000000..f9647f7b --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -0,0 +1,177 @@ +#include "LowSideCurrentSense.h" +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + +// Inline sensor init function +void LowSideCurrentSense::init(){ + // configure ADC variables + _configure3PinsDMA(pinA,pinB,pinC); + _start3PinsDMA(); //start next acuisition + // calibrate zero offsets + // calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowSideCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + offset_ia += _readADCVoltage(pinA); + offset_ib += _readADCVoltage(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps + // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps + // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + _start3PinsDMA(); + + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowSideCurrentSense::driverSync(BLDCDriver *driver){ + return 1; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int LowSideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + int exit_flag = 1; + if(skip_align) return exit_flag; + + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = fabs(c.a / c.b); + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if( ab_ratio > 1.5 ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4*c1.a; + c.b = c.b*0.6 + 0.4*c1.b; + c.c = c.c*0.6 + 0.4*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = fabs(c.b/c.a); + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if( ba_ratio > 1.5 ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // if phase C measured + if(_isset(pinC)){ + // set phase B active and phases A and C down + driver->setPwm(0, 0, voltage); + _delay(200); + c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 50; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.c = (c.c+c1.c)/50.0; + } + driver->setPwm(0, 0, 0); + gain_c *= _sign(c.c); + } + + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + return exit_flag; +} + diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h new file mode 100644 index 00000000..48c6dc80 --- /dev/null +++ b/src/current_sense/LowSideCurrentSense.h @@ -0,0 +1,57 @@ +#ifndef LOW_SIDE_CS_LIB_H +#define LOW_SIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/CurrentSense.h" +#include "hardware_api.h" + +class LowSideCurrentSense: public CurrentSense{ + public: + /** + LowSideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + double shunt_resistor; //!< Shunt resistor value + double amp_gain; //!< amp gain value + double volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + double offset_ia; //!< zero current A voltage value (center of the adc reading) + double offset_ib; //!< zero current B voltage value (center of the adc reading) + double offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 4c4c1086..9ca93ae7 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,23 +20,4 @@ float _readADCVoltageInline(const int pinA); */ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - adc pin A - * @param pinB - adc pin B - * @param pinC - adc pin C - */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - the arduino pin to be read (it has to be ADC pin) - */ -float _readADCVoltageLowSide(const int pinA); - -/** - * function syncing the Driver with the ADC for the LowSide Sensing - */ -void _driverSyncLowSide(); #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..fb0dbeaf --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,266 @@ + +#include "../hardware_api.h" + +// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless +static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout +static uint32_t ADC_FirstPin; // PA04 +static uint32_t ADC_LastPin; // PA06 +static uint32_t BufferSize = 0; + +uint16_t adcBuffer[20]; + + +static uint32_t ADC_pinA = A4; +static uint32_t ADC_pinB = A5; +static uint32_t ADC_pinC = 8; + + +#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val +#define _ADC_RESOLUTION (1 << 12) +#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) + +void adc_dma(void *rxdata, size_t hwords); +void adc_init(); +void adc_start_with_DMA(); +void dma_init(); + +void _start3PinsDMA() +{ + adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); + adc_start_with_DMA(); +} +void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +{ + while(ADC->CTRLA.bit.ENABLE) ; + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[adcA] * ADC_CONV_; + b = adcBuffer[adcB] * ADC_CONV_; + if(_isset(pinC)) + { + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[adcC] * ADC_CONV_; + } +} + +// function reading an ADC value and returning the read voltage +void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ + + uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; + + pinMode(42, INPUT); + ADC_FirstPin = min(adcA, adcB); + ADC_LastPin = max(adcA, adcB); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) + { + pinMode(pinC, INPUT); + ADC_FirstPin = min(ADC_FirstPin, adcC); + ADC_LastPin = max(ADC_LastPin, adcC); + } + + ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout + BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + + ADC_pinA = pinA; + ADC_pinB = pinB; + ADC_pinC = pinC; + + // ADC and DMA + adc_init(); + dma_init(); +} + + + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) + +static uint32_t ADC_DMA_chnl = 3; // DMA channel + + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +/** + * @brief Initialize ADC + * @retval void + */ +void adc_init(){ + + auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X + // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + ADCsync(); // ref 31.6.16 + + a = analogRead(ADC_pinA); // do some pin init pinPeripheral() + b = analogRead(ADC_pinB); // do some pin init pinPeripheral() + c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + SerialUSB.println("--------------"); + SerialUSB.println(a); + SerialUSB.println(b); + SerialUSB.println(c); + SerialUSB.println("--------------"); +/* +Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan +This register gives the number of input sources included in the pin scan. The number of input sources included is +INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + +INPUTOFFSET + INPUTSCAN. +The range of the scan mode must not exceed the number of input channels available on the device. +Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection +These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If +the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit +group in the SamplingControl register must be written. +Table 32-14. Positive Mux Input Selection +MUXPOS[4:0] Group configuration Description +0x00 PIN0 ADC AIN0 pin +0x01 PIN1 ADC AIN1 pin +0x02 PIN2 ADC AIN2 pin +0x03 PIN3 ADC AIN3 pin +0x04 PIN4 ADC AIN4 pin +0x05 PIN5 ADC AIN5 pin +0x06 PIN6 ADC AIN6 pin +0x07 PIN7 ADC AIN7 pin +0x08 PIN8 ADC AIN8 pin +0x09 PIN9 ADC AIN9 pin +0x0A PIN10 ADC AIN10 pin +0x0B PIN11 ADC AIN11 pin +0x0C PIN12 ADC AIN12 pin +0x0D PIN13 ADC AIN13 pin +0x0E PIN14 ADC AIN14 pin +0x0F PIN15 ADC AIN15 pin +0x10 PIN16 ADC AIN16 pin +0x11 PIN17 ADC AIN17 pin +0x12 PIN18 ADC AIN18 pin +0x13 PIN19 ADC AIN19 pin +0x14-0x17 Reserved +0x18 TEMP Temperature reference +0x19 BANDGAP Bandgap voltage +0x1A SCALEDCOREVCC 1/4 scaled core supply +0x1B SCALEDIOVCC 1/4 scaled I/O supply +0x1C DAC DAC output +0x1D-0x1F Reserved +*/ + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + + +/** + * @brief dma_init + * @retval void + */ +void dma_init() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + +/** + * @brief adc_dma + * @retval void + */ +void adc_dma(void *rxdata, size_t hwords) { + uint32_t temp_CHCTRLB_reg; + + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); + temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | +DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + +/** + * @brief adc_stop_with_DMA + * @retval void + */ +void adc_stop_with_DMA(void) +{ + ADC->CTRLA.bit.ENABLE = 0x00; + // SerialUSB.println("DMA stopped!"); +} + +/** + * @brief adc_start_with_DMA + * @retval void + */ +void adc_start_with_DMA(void) +{ + // SerialUSB.println("strating DMA..."); + + + ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADC->SWTRIG.bit.FLUSH = 1; + ADC->CTRLA.bit.ENABLE = 0x01; +} + + +/** + * @brief DMAC_Handler + * @retval void + */ +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adc_stop_with_DMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; +} From 7d3c011770301f5a27e00ddb94e42df4749cdb0e Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 14 May 2021 20:26:26 -0600 Subject: [PATCH 182/749] wrapped samd21 adc/dma implementation in an ugly class --- src/SimpleFOC.h | 1 + src/current_sense/LowSideCurrentSense.cpp | 13 +- src/current_sense/LowSideCurrentSense.h | 3 + .../hardware_specific/samd21_mcu.cpp | 208 ++++++++---------- .../hardware_specific/samd21_mcu.h | 60 +++++ 5 files changed, 168 insertions(+), 117 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index f9647f7b..0c12e528 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -5,7 +5,9 @@ // - phA - A phase adc pin // - phB - B phase adc pin // - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ +LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): +adc(_pinA, _pinB, _pinC) +{ pinA = _pinA; pinB = _pinB; pinC = _pinC; @@ -21,9 +23,7 @@ LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int // Inline sensor init function void LowSideCurrentSense::init(){ - // configure ADC variables - _configure3PinsDMA(pinA,pinB,pinC); - _start3PinsDMA(); //start next acuisition + adc.init(); // calibrate zero offsets // calibrateOffsets(); } @@ -54,8 +54,9 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - _read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - _start3PinsDMA(); + + adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); + adc._start3PinsDMA(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 48c6dc80..2a898bfc 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -6,6 +6,7 @@ #include "../common/time_utils.h" #include "../common/base_classes/CurrentSense.h" #include "hardware_api.h" +#include "hardware_specific/samd21_mcu.h" class LowSideCurrentSense: public CurrentSense{ public: @@ -52,6 +53,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) + SAMDCurrentSensceADC adc; + }; #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index fb0dbeaf..c95bb754 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,35 +1,57 @@ -#include "../hardware_api.h" +#include "samd21_mcu.h" -// this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -static uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout -static uint32_t ADC_FirstPin; // PA04 -static uint32_t ADC_LastPin; // PA06 -static uint32_t BufferSize = 0; -uint16_t adcBuffer[20]; +void adc_stop_with_DMA(void); +void adc_start_with_DMA(void); +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; +} dmacdescriptor ; +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); +dmacdescriptor descriptor __attribute__ ((aligned (16))); +DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -static uint32_t ADC_pinA = A4; -static uint32_t ADC_pinB = A5; -static uint32_t ADC_pinC = 8; -#define _ADC_VOLTAGE 3.3 // we use ADC_REFCTRL_REFSEL_INTVCC1_Val -#define _ADC_RESOLUTION (1 << 12) -#define ADC_CONV_ ( _ADC_VOLTAGE / _ADC_RESOLUTION ) +SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) +: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) +{ + ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; +} + +void SAMDCurrentSensceADC::init() +{ + _configure3PinsDMA(); + _start3PinsDMA(); //s +} -void adc_dma(void *rxdata, size_t hwords); -void adc_init(); -void adc_start_with_DMA(); -void dma_init(); -void _start3PinsDMA() +void SAMDCurrentSensceADC::_start3PinsDMA() { adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); adc_start_with_DMA(); } -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) +void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) { while(ADC->CTRLA.bit.ENABLE) ; uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; @@ -44,7 +66,7 @@ void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, floa } // function reading an ADC value and returning the read voltage -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ +void SAMDCurrentSensceADC::_configure3PinsDMA(){ uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; @@ -65,10 +87,6 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; - ADC_pinA = pinA; - ADC_pinB = pinB; - ADC_pinC = pinC; - // ADC and DMA adc_init(); dma_init(); @@ -76,41 +94,17 @@ void _configure3PinsDMA(const int pinA,const int pinB,const int pinC){ -// ADC DMA sequential free running (6) with Interrupts ///////////////// - -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) - -static uint32_t ADC_DMA_chnl = 3; // DMA channel -/** - * @brief ADC sync wait - * @retval void - */ -static __inline__ void ADCsync() __attribute__((always_inline, unused)); -static void ADCsync() { - while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free -} - /** * @brief Initialize ADC * @retval void */ -void adc_init(){ +void SAMDCurrentSensceADC::adc_init(){ - auto a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - auto b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - auto c = analogRead(ADC_pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); @@ -121,54 +115,46 @@ void adc_init(){ // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 - a = analogRead(ADC_pinA); // do some pin init pinPeripheral() - b = analogRead(ADC_pinB); // do some pin init pinPeripheral() - c = analogRead(ADC_pinC); // do some pin init pinPeripheral() - SerialUSB.println("--------------"); - SerialUSB.println(a); - SerialUSB.println(b); - SerialUSB.println(c); - SerialUSB.println("--------------"); -/* -Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan -This register gives the number of input sources included in the pin scan. The number of input sources included is -INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + -INPUTOFFSET + INPUTSCAN. -The range of the scan mode must not exceed the number of input channels available on the device. -Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection -These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If -the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit -group in the SamplingControl register must be written. -Table 32-14. Positive Mux Input Selection -MUXPOS[4:0] Group configuration Description -0x00 PIN0 ADC AIN0 pin -0x01 PIN1 ADC AIN1 pin -0x02 PIN2 ADC AIN2 pin -0x03 PIN3 ADC AIN3 pin -0x04 PIN4 ADC AIN4 pin -0x05 PIN5 ADC AIN5 pin -0x06 PIN6 ADC AIN6 pin -0x07 PIN7 ADC AIN7 pin -0x08 PIN8 ADC AIN8 pin -0x09 PIN9 ADC AIN9 pin -0x0A PIN10 ADC AIN10 pin -0x0B PIN11 ADC AIN11 pin -0x0C PIN12 ADC AIN12 pin -0x0D PIN13 ADC AIN13 pin -0x0E PIN14 ADC AIN14 pin -0x0F PIN15 ADC AIN15 pin -0x10 PIN16 ADC AIN16 pin -0x11 PIN17 ADC AIN17 pin -0x12 PIN18 ADC AIN18 pin -0x13 PIN19 ADC AIN19 pin -0x14-0x17 Reserved -0x18 TEMP Temperature reference -0x19 BANDGAP Bandgap voltage -0x1A SCALEDCOREVCC 1/4 scaled core supply -0x1B SCALEDIOVCC 1/4 scaled I/O supply -0x1C DAC DAC output -0x1D-0x1F Reserved -*/ + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; ADCsync(); ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] @@ -176,7 +162,8 @@ MUXPOS[4:0] Group configuration Description ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; ADCsync(); @@ -187,7 +174,7 @@ MUXPOS[4:0] Group configuration Description * @brief dma_init * @retval void */ -void dma_init() { +void SAMDCurrentSensceADC::dma_init() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -201,7 +188,7 @@ void dma_init() { * @brief adc_dma * @retval void */ -void adc_dma(void *rxdata, size_t hwords) { +void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { uint32_t temp_CHCTRLB_reg; DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); @@ -209,7 +196,7 @@ void adc_dma(void *rxdata, size_t hwords) { DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | -DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -224,6 +211,9 @@ DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } + + + /** * @brief adc_stop_with_DMA * @retval void @@ -241,16 +231,12 @@ void adc_stop_with_DMA(void) void adc_start_with_DMA(void) { // SerialUSB.println("strating DMA..."); - - - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; + // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } - - /** * @brief DMAC_Handler * @retval void @@ -263,4 +249,4 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..fb5d319a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,60 @@ + +#include "../hardware_api.h" + + +class SAMDCurrentSensceADC +{ + +public: + + SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + + void init(); + // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout + uint32_t ADC_FirstPin; // PA04 + uint32_t ADC_LastPin; // PA06 + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA = A4; + uint32_t pinB = A5; + uint32_t pinC = 8; + + float _ADC_VOLTAGE; + float _ADC_RESOLUTION; + float ADC_CONV_; + + void _start3PinsDMA(); + void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); + // function reading an ADC value and returning the read voltage + void _configure3PinsDMA(); + + + + + uint32_t ADC_DMA_chnl = 3; // DMA channel + + + /** + * @brief Initialize ADC + * @retval void + */ + void adc_init(); + + + /** + * @brief dma_init + * @retval void + */ + void dma_init(); + + /** + * @brief adc_dma + * @retval void + */ + void adc_dma(void *rxdata, size_t hwords); + +}; From a5c7528c102dbea2f3b64206592078a57aff146a Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 15 May 2021 11:13:58 -0600 Subject: [PATCH 183/749] SAMDCurrentSenseADCDMA class --- src/current_sense/LowSideCurrentSense.cpp | 4 +- src/current_sense/LowSideCurrentSense.h | 2 +- .../hardware_specific/samd21_mcu.cpp | 157 ++++++------------ .../hardware_specific/samd21_mcu.h | 76 ++++----- 4 files changed, 91 insertions(+), 148 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 0c12e528..7f6b3ceb 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,8 +55,8 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc._read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c); - adc._start3PinsDMA(); + adc.readResults(current.a, current.b, current.c); + adc.startADCScan(); return current; } diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 2a898bfc..379e441c 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -53,7 +53,7 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - SAMDCurrentSensceADC adc; + SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index c95bb754..f511ec1d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,9 +1,10 @@ #include "samd21_mcu.h" + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -void adc_stop_with_DMA(void); -void adc_start_with_DMA(void); +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); /** * @brief ADC sync wait @@ -16,91 +17,66 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; -} dmacdescriptor ; -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); -dmacdescriptor descriptor __attribute__ ((aligned (16))); -DmacDescriptor *desc; // DMA descriptor address (so we can change contents) -SAMDCurrentSensceADC::SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage, uint32_t adcBits) -: _ADC_VOLTAGE(arefaVoltage), _ADC_RESOLUTION(1 << adcBits) + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) { - ADC_CONV_ = ( _ADC_VOLTAGE / _ADC_RESOLUTION ); - this->pinA = pinA; - this->pinB = pinB; - this->pinC = pinC; + countsToVolts = ( voltageAREF / maxCountsADC ); } -void SAMDCurrentSensceADC::init() -{ - _configure3PinsDMA(); - _start3PinsDMA(); //s +void SAMDCurrentSenseADCDMA::init(){ + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() } -void SAMDCurrentSensceADC::_start3PinsDMA() -{ - adc_dma(adcBuffer + ADC_OneBeforeFirstPin, BufferSize); - adc_start_with_DMA(); +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); } -void SAMDCurrentSensceADC::_read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c) -{ + +void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ while(ADC->CTRLA.bit.ENABLE) ; - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[adcA] * ADC_CONV_; - b = adcBuffer[adcB] * ADC_CONV_; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + a = adcBuffer[ainA] * countsToVolts; + b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[adcC] * ADC_CONV_; + c = adcBuffer[ainC] * countsToVolts; } } -// function reading an ADC value and returning the read voltage -void SAMDCurrentSensceADC::_configure3PinsDMA(){ +void SAMDCurrentSenseADCDMA::initPins(){ - uint32_t adcA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t adcB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t adcC = g_APinDescription[pinC].ulADCChannelNumber; - - pinMode(42, INPUT); - ADC_FirstPin = min(adcA, adcB); - ADC_LastPin = max(adcA, adcB); + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); if( _isset(pinC) ) { pinMode(pinC, INPUT); - ADC_FirstPin = min(ADC_FirstPin, adcC); - ADC_LastPin = max(ADC_LastPin, adcC); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); } - ADC_OneBeforeFirstPin = ADC_FirstPin - 1; //hack to discard noisy first readout - BufferSize = ADC_LastPin - ADC_OneBeforeFirstPin + 1; + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; - // ADC and DMA - adc_init(); - dma_init(); } - - - - -/** - * @brief Initialize ADC - * @retval void - */ -void SAMDCurrentSensceADC::adc_init(){ +void SAMDCurrentSenseADCDMA::initADC(){ analogRead(pinA); // do some pin init pinPeripheral() analogRead(pinB); // do some pin init pinPeripheral() @@ -155,9 +131,9 @@ void SAMDCurrentSensceADC::adc_init(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; // so the adc will scan from AIN[1] to AIN[ADC_Number+1] + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -169,12 +145,9 @@ void SAMDCurrentSensceADC::adc_init(){ ADCsync(); } +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -/** - * @brief dma_init - * @retval void - */ -void SAMDCurrentSensceADC::dma_init() { +void SAMDCurrentSenseADCDMA::initDMA() { // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -184,68 +157,46 @@ void SAMDCurrentSensceADC::dma_init() { DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); } -/** - * @brief adc_dma - * @retval void - */ -void SAMDCurrentSensceADC::adc_dma(void *rxdata, size_t hwords) { - uint32_t temp_CHCTRLB_reg; - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl)); - temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) | - DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHCTRLB.reg = temp_CHCTRLB_reg; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; descriptor.btcnt = hwords; descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor)); + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); // start channel - DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl); + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } - - -/** - * @brief adc_stop_with_DMA - * @retval void - */ -void adc_stop_with_DMA(void) -{ +void adcStopWithDMA(void){ ADC->CTRLA.bit.ENABLE = 0x00; - // SerialUSB.println("DMA stopped!"); } -/** - * @brief adc_start_with_DMA - * @retval void - */ -void adc_start_with_DMA(void) -{ - // SerialUSB.println("strating DMA..."); - // ADC->INPUTCTRL.bit.MUXPOS = ADC_OneBeforeFirstPin; - // ADC->INPUTCTRL.bit.INPUTSCAN = ADC_LastPin; +void adcStartWithDMA(void){ ADC->INPUTCTRL.bit.INPUTOFFSET = 0; ADC->SWTRIG.bit.FLUSH = 1; ADC->CTRLA.bit.ENABLE = 0x01; } -/** - * @brief DMAC_Handler - * @retval void - */ + void DMAC_Handler() { uint8_t active_channel; active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adc_stop_with_DMA(); + adcStopWithDMA(); DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index fb5d319a..c410b522 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,60 +1,52 @@ #include "../hardware_api.h" - -class SAMDCurrentSensceADC + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + +class SAMDCurrentSenseADCDMA { public: - SAMDCurrentSensceADC(int pinA, int pinB, int pinC, float arefaVoltage = 3.3, uint32_t adcBits = 12); + SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void init(); - // this code was pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless - uint32_t ADC_OneBeforeFirstPin; // hack to discard first noisy readout - uint32_t ADC_FirstPin; // PA04 - uint32_t ADC_LastPin; // PA06 - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA = A4; - uint32_t pinB = A5; - uint32_t pinC = 8; - - float _ADC_VOLTAGE; - float _ADC_RESOLUTION; - float ADC_CONV_; + void startADCScan(); + void readResults(float & a, float & b, float & c); - void _start3PinsDMA(); - void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); - // function reading an ADC value and returning the read voltage - void _configure3PinsDMA(); +private: + void adcToDMATransfer(void *rxdata, size_t hwords); + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; - - uint32_t ADC_DMA_chnl = 3; // DMA channel - - - /** - * @brief Initialize ADC - * @retval void - */ - void adc_init(); + uint16_t adcBuffer[20]; - /** - * @brief dma_init - * @retval void - */ - void dma_init(); + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel - /** - * @brief adc_dma - * @retval void - */ - void adc_dma(void *rxdata, size_t hwords); + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); }; From 7e802f682927c54b2490beadb9057295b8d7a12c Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 08:32:25 -0600 Subject: [PATCH 184/749] FEAT ADC-DMA fix deadlock --- src/current_sense/LowSideCurrentSense.cpp | 7 +++++-- src/current_sense/LowSideCurrentSense.h | 3 ++- .../hardware_specific/samd21_mcu.cpp | 18 +++++++++++++----- .../hardware_specific/samd21_mcu.h | 6 +++--- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp index 7f6b3ceb..46110c34 100644 --- a/src/current_sense/LowSideCurrentSense.cpp +++ b/src/current_sense/LowSideCurrentSense.cpp @@ -55,10 +55,13 @@ PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - adc.readResults(current.a, current.b, current.c); + if(adc.readResults(current.a, current.b, current.c)) + { + oldCurrent = current; + } adc.startADCScan(); - return current; + return oldCurrent; } // Function synchronizing current sense with motor driver. // for in-line sensig no such thing is necessary diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h index 379e441c..461f3e35 100644 --- a/src/current_sense/LowSideCurrentSense.h +++ b/src/current_sense/LowSideCurrentSense.h @@ -52,7 +52,8 @@ class LowSideCurrentSense: public CurrentSense{ double offset_ia; //!< zero current A voltage value (center of the adc reading) double offset_ib; //!< zero current B voltage value (center of the adc reading) double offset_ic; //!< zero current C voltage value (center of the adc reading) - + + PhaseCurrent_s oldCurrent; SAMDCurrentSenseADCDMA adc; }; diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f511ec1d..e9f43ef7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -40,17 +40,19 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -void SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ - while(ADC->CTRLA.bit.ENABLE) ; +bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; a = adcBuffer[ainA] * countsToVolts; b = adcBuffer[ainB] * countsToVolts; if(_isset(pinC)) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; c = adcBuffer[ainC] * countsToVolts; } + return true; } void SAMDCurrentSenseADCDMA::initPins(){ @@ -61,11 +63,11 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); @@ -158,7 +160,7 @@ void SAMDCurrentSenseADCDMA::initDMA() { } -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; @@ -183,13 +185,19 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, size_t hwords) { void adcStopWithDMA(void){ + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; + } void adcStartWithDMA(void){ + ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); } void DMAC_Handler() { diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c410b522..1c1603b9 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,6 +1,6 @@ #include "../hardware_api.h" - +#include typedef struct { uint16_t btctrl; uint16_t btcnt; @@ -18,11 +18,11 @@ class SAMDCurrentSenseADCDMA void init(); void startADCScan(); - void readResults(float & a, float & b, float & c); + bool readResults(float & a, float & b, float & c); private: - void adcToDMATransfer(void *rxdata, size_t hwords); + void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); From bb7d29c655cd96bfdaf72e9103c10c5f41d87aff Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 18 May 2021 12:10:46 -0600 Subject: [PATCH 185/749] FEAT add _startADC3PinConversionLowSide() API --- src/SimpleFOC.h | 2 +- src/current_sense/LowSideCurrentSense.cpp | 181 ------------------ src/current_sense/LowSideCurrentSense.h | 61 ------ src/current_sense/LowsideCurrentSense.cpp | 2 + src/current_sense/hardware_api.h | 24 ++- .../hardware_specific/esp32_mcu.cpp | 3 + .../hardware_specific/samd21_mcu.cpp | 115 +++++++++-- .../hardware_specific/samd21_mcu.h | 23 ++- 8 files changed, 147 insertions(+), 264 deletions(-) delete mode 100644 src/current_sense/LowSideCurrentSense.cpp delete mode 100644 src/current_sense/LowSideCurrentSense.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 02e215b9..4f9f1108 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,7 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" -#include "current_sense/LowSideCurrentSense.h" +#include "current_sense/LowsideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowSideCurrentSense.cpp b/src/current_sense/LowSideCurrentSense.cpp deleted file mode 100644 index 46110c34..00000000 --- a/src/current_sense/LowSideCurrentSense.cpp +++ /dev/null @@ -1,181 +0,0 @@ -#include "LowSideCurrentSense.h" -// InlineCurrentSensor constructor -// - shunt_resistor - shunt resistor value -// - gain - current-sense op-amp gain -// - phA - A phase adc pin -// - phB - B phase adc pin -// - phC - C phase adc pin (optional) -LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC): -adc(_pinA, _pinB, _pinC) -{ - pinA = _pinA; - pinB = _pinB; - pinC = _pinC; - - shunt_resistor = _shunt_resistor; - amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps - // gains for each phase - gain_a = volts_to_amps_ratio; - gain_b = volts_to_amps_ratio; - gain_c = volts_to_amps_ratio; -} - -// Inline sensor init function -void LowSideCurrentSense::init(){ - adc.init(); - // calibrate zero offsets - // calibrateOffsets(); -} -// Function finding zero offsets of the ADC -void LowSideCurrentSense::calibrateOffsets(){ - const int calibration_rounds = 1000; - - // find adc offset = zero current voltage - offset_ia = 0; - offset_ib = 0; - offset_ic = 0; - // read the adc voltage 1000 times ( arbitrary number ) - for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltage(pinA); - offset_ib += _readADCVoltage(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); - _delay(1); - } - // calculate the mean offsets - offset_ia = offset_ia / calibration_rounds; - offset_ib = offset_ib / calibration_rounds; - if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; -} - -// read all three phase currents (if possible 2 or 3) -PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){ - PhaseCurrent_s current; - // current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps - // current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - // current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps - - if(adc.readResults(current.a, current.b, current.c)) - { - oldCurrent = current; - } - adc.startADCScan(); - - return oldCurrent; -} -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int LowSideCurrentSense::driverSync(BLDCDriver *driver){ - return 1; -} - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int LowSideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ - int exit_flag = 1; - if(skip_align) return exit_flag; - - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = fabs(c.a / c.b); - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5 ){ // should be ~2 - gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = fabs(c.b/c.a); - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5 ){ // should be ~2 - gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - - // if phase C measured - if(_isset(pinC)){ - // set phase B active and phases A and C down - driver->setPwm(0, 0, voltage); - _delay(200); - c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 50; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0; - } - driver->setPwm(0, 0, 0); - gain_c *= _sign(c.c); - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - return exit_flag; -} - diff --git a/src/current_sense/LowSideCurrentSense.h b/src/current_sense/LowSideCurrentSense.h deleted file mode 100644 index 461f3e35..00000000 --- a/src/current_sense/LowSideCurrentSense.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef LOW_SIDE_CS_LIB_H -#define LOW_SIDE_CS_LIB_H - -#include "Arduino.h" -#include "../common/foc_utils.h" -#include "../common/time_utils.h" -#include "../common/base_classes/CurrentSense.h" -#include "hardware_api.h" -#include "hardware_specific/samd21_mcu.h" - -class LowSideCurrentSense: public CurrentSense{ - public: - /** - LowSideCurrentSense class constructor - @param shunt_resistor shunt resistor value - @param gain current-sense op-amp gain - @param phA A phase adc pin - @param phB B phase adc pin - @param phC C phase adc pin (optional) - */ - LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); - - // CurrentSense interface implementing functions - void init() override; - PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; - - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - private: - - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - - // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio - - /** - * Function finding zero offsets of the ADC - */ - void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) - - PhaseCurrent_s oldCurrent; - SAMDCurrentSenseADCDMA adc; - -}; - -#endif \ No newline at end of file diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index abebf03d..e0df940a 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -34,6 +34,7 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ic= 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < 1000; i++) { + _startADC3PinConversionLowSide(); offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); @@ -48,6 +49,7 @@ void LowsideCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; + _startADC3PinConversionLowSide(); current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 8a743bf0..7b648cde 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -20,8 +20,26 @@ float _readADCVoltageInline(const int pinA); */ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); -void _start3PinsDMA(); -void _read3PinsDMA(const int pinA,const int pinB,const int pinC, float & a, float & b, float & c); -void _configure3PinsDMA(const int pinA,const int pinB,const int pinC = NOT_SET); +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); +void _startADC3PinConversionLowSide(); + +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA); + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide(); #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 7f84f9a9..3f7e83b3 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -30,6 +30,9 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } +void _startADC3PinConversionLowSide(){ + +} // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index e9f43ef7..5c5de779 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,5 +1,69 @@ #include "samd21_mcu.h" +#include "../hardware_api.h" + + +static bool freeRunning = false; +static int _pinA, _pinB, _pinC; +static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode +static SAMDCurrentSenseADCDMA instance; +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC) +{ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} +void _startADC3PinConversionLowSide() +{ + instance.startADCScan(); +} +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA) +{ + instance.readResults(a, b, c); + + if(pinA == _pinA) + return instance.toVolts(a); + if(pinA == _pinB) + return instance.toVolts(b); + if(pinA == _pinC) + return instance.toVolts(c); + + return NAN; +} + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide() +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); + instance.startADCScan(); + //TODO: hook with PWM interrupts +} + + + + + + + + + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless @@ -17,17 +81,27 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +{ + + return &instance; +} +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +{ +} - - -SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) -: pinA(pinA), pinB(pinB), pinC(pinC), pinAREF(pinAREF), channelDMA(channelDMA), voltageAREF(voltageAREF), maxCountsADC(1 << adcBits) +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) { + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; + this->pinAREF = pinAREF; + this->channelDMA = channelDMA; + this->voltageAREF = voltageAREF; + this->maxCountsADC = 1 << adcBits; countsToVolts = ( voltageAREF / maxCountsADC ); -} -void SAMDCurrentSenseADCDMA::init(){ initPins(); initADC(); initDMA(); @@ -40,21 +114,26 @@ void SAMDCurrentSenseADCDMA::startADCScan(){ adcStartWithDMA(); } -bool SAMDCurrentSenseADCDMA::readResults(float & a, float & b, float & c){ +bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ if(ADC->CTRLA.bit.ENABLE) return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[ainA] * countsToVolts; - b = adcBuffer[ainB] * countsToVolts; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; if(_isset(pinC)) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[ainC] * countsToVolts; + c = adcBuffer[ainC]; } return true; } + +float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { + return counts * countsToVolts; +} + void SAMDCurrentSenseADCDMA::initPins(){ pinMode(pinAREF, INPUT); @@ -184,10 +263,23 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { } +int iii = 0; + void adcStopWithDMA(void){ ADCsync(); ADC->CTRLA.bit.ENABLE = 0x00; - + // ADCsync(); + // if(iii++ % 1000 == 0) + // { + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); + // } + + } void adcStartWithDMA(void){ @@ -208,4 +300,5 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + } \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index 1c1603b9..a01c72e3 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,5 +1,11 @@ +#ifndef CURRENT_SENSE_SAMD21_H +#define CURRENT_SENSE_SAMD21_H + +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif -#include "../hardware_api.h" #include typedef struct { uint16_t btctrl; @@ -9,17 +15,17 @@ uint32_t descaddr; } dmacdescriptor ; + class SAMDCurrentSenseADCDMA { public: - - SAMDCurrentSenseADCDMA(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); - - void init(); + static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); + SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); void startADCScan(); - bool readResults(float & a, float & b, float & c); - + bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + float toVolts(uint16_t counts); private: void adcToDMATransfer(void *rxdata, uint32_t hwords); @@ -41,6 +47,7 @@ class SAMDCurrentSenseADCDMA uint32_t pinC; uint32_t pinAREF; uint32_t channelDMA; // DMA channel + bool freeRunning; float voltageAREF; float maxCountsADC; @@ -50,3 +57,5 @@ class SAMDCurrentSenseADCDMA dmacdescriptor descriptor __attribute__ ((aligned (16))); }; + +#endif \ No newline at end of file From dce977457ea735d9c18d702ff3187e2b7302c2e4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 15:09:43 +0200 Subject: [PATCH 186/749] remove oddities in MagneticSensorSPI - fix issue #82 --- src/sensors/MagneticSensorSPI.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b3d82dee..5ee9ecf9 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -75,11 +75,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif + // do any architectures need to set the clock divider for SPI? Why was this in the code? + //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); // velocity calculation init @@ -170,35 +167,27 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); #if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word From e3bd3ac33366025c6d903c5b64edc58a7494bbd2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 22:37:07 +0200 Subject: [PATCH 187/749] sensor precision fix, part 1 --- src/common/base_classes/Sensor.cpp | 60 ++++++++++++++++++-------- src/common/base_classes/Sensor.h | 69 ++++++++++++++++++++++++++---- src/sensors/MagneticSensorI2C.cpp | 43 +------------------ src/sensors/MagneticSensorI2C.h | 14 +----- src/sensors/MagneticSensorSPI.cpp | 51 +--------------------- src/sensors/MagneticSensorSPI.h | 4 +- 6 files changed, 108 insertions(+), 133 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..93f4e7fe 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,31 +2,53 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** - * returns 0 if it does need search for absolute zero - * 0 - magnetic sensor (& encoder with index which is found) - * 1 - ecoder with index (with index not found yet) - */ -int Sensor::needsSearch(){ - return 0; + +float Sensor::getAngle() { + float val = getShaftAngle(); + angle_prev_ts = _micros(); + float d_angle = val - angle_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; + angle_prev = val; + return val; } - /** get current angular velocity (rad/s)*/ -float Sensor::getVelocity(){ + + /** get current angular velocity (rad/s)*/ +float Sensor::getVelocity() { // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - + float vel = (angle_prev - vel_angle_prev)/Ts; // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; + vel_angle_prev = angle_prev; + vel_angle_prev_ts = angle_prev_ts; return vel; +} + + + +float Sensor::getPosition(){ + return (float)full_rotations * _2PI + angle_prev; +} + + + +double Sensor::getPrecisePosition() { + return (double)full_rotations * (double)_2PI + (double)angle_prev; +} + + + +int32_t Sensor::getFullRotations() { + return full_rotations; +} + + + +int Sensor::needsSearch() { + return 0; // default false } \ No newline at end of file diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 3b219401..32ba7b25 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -1,6 +1,8 @@ #ifndef SENSOR_H #define SENSOR_H +#include + /** * Direction structure */ @@ -20,27 +22,76 @@ enum Pullup{ }; /** - * Sensor abstract class defintion - * Each sensor needs to have these functions implemented + * Sensor abstract class defintion + * + * This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have + * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the + * most basic common features so that a FOC driver can obtain the data it needs for operation. + * + * To implement your own sensors, create a sub-class of this class, and implement the getAngle() + * method. getAngle() returns a float value, in radians, representing the current shaft angle in the + * range 0 to 2*PI (one full turn). + * + * To function correctly, the sensor class getAngle() method has to be called sufficiently quickly. Normally, + * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly + * enough, both for correct motor and sensor operation. + * + * The Sensor base class provides an implementation of getVelocity(), and takes care of counting full + * revolutions in a precise way, but if you wish you can additionally override these methods to provide more + * optimal implementations for your hardware. + * */ class Sensor{ public: - - /** get current angle (rad) */ - virtual float getAngle()=0; - /** get current angular velocity (rad/s)*/ + /** + * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + */ + virtual float getAngle(); + /** + * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + * This method is pure virtual and must be implemented in subclasses. + * Calling this method directly does not update the base-class internal fields. + * Use getAngle() when calling from other code. + */ + virtual float getShaftAngle()=0; + /** + * Get current angular velocity (rad/s) + * Can be overridden in subclasses. Base implementation uses the values + * previously returned by getShaftAngle(). + */ virtual float getVelocity(); + /** + * Get current position (in rad) including full rotations and shaft angle + * Note that this value has limited precision as the number of rotations increases, + * as the limited precision of float can't capture the large angle of the full rotations + * and the small angle of the shaft angle at the same time. + */ + virtual float getPosition(); + + /** + * On architectures supporting it, this will return a double precision position value + */ + virtual double getPrecisePosition(); + + /** + * Get the number of full rotations + */ + virtual int32_t getFullRotations(); + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) */ virtual int needsSearch(); - private: + protected: // velocity calculation variables - float angle_prev=0; //!< angle in previous velocity calculation step - long velocity_calc_timestamp=0; //!< last velocity calculation timestamp + float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity + long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity + float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity + long vel_angle_prev_ts=0; //!< last velocity calculation timestamp + int32_t full_rotations=0; // full rotation tracking }; #endif diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index c9029f6e..7e2bc6c2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -64,54 +64,15 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // I2C communication begin wire->begin(); - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - // return the full angle +float MagneticSensorI2C::getShaftAngle(){ // (number of full rotations)*2PI + current sensor angle - return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; + return ( getRawCount() / (float)cpr) * _2PI ; } -// Shaft velocity calculation -float MagneticSensorI2C::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; -} // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 9966d6eb..f5119643 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -46,9 +46,7 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getShaftAngle() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); @@ -72,15 +70,7 @@ class MagneticSensorI2C: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //!begin(); // do any architectures need to set the clock divider for SPI? Why was this in the code? //spi->setClockDivider(SPI_CLOCK_DIV8); - digitalWrite(chip_select_pin, HIGH); - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - - // return the full angle - // (number of full rotations)*2PI + current sensor angle - return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; -} - -// Shaft velocity calculation -float MagneticSensorSPI::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; +float MagneticSensorSPI::getShaftAngle(){ + return (getRawCount() / (float)cpr) * _2PI; } - // function reading the raw counter of the magnetic sensor int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 77fcde4d..de5dea50 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -44,9 +44,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getShaftAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; From cb47fd0ae39cc1dcc0304c25e67c7427c47a1dda Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 22:40:23 +0200 Subject: [PATCH 188/749] mask out SAMD21 current sensing when compiling on other platforms --- src/current_sense/hardware_specific/samd21_mcu.cpp | 6 +++++- src/current_sense/hardware_specific/samd21_mcu.h | 6 ++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 5c5de779..8bb6d38c 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,3 +1,4 @@ +#ifdef _SAMD21_ #include "samd21_mcu.h" #include "../hardware_api.h" @@ -301,4 +302,7 @@ void DMAC_Handler() { DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; -} \ No newline at end of file +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index a01c72e3..c0cec74a 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,3 +1,5 @@ +#ifdef _SAMD21_ + #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H @@ -58,4 +60,8 @@ class SAMDCurrentSenseADCDMA }; +#endif + + + #endif \ No newline at end of file From fef978238fbdce6b2a4c109d3a9305823eb9a6a4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 30 May 2021 23:18:59 +0200 Subject: [PATCH 189/749] sensor precision fix, part 2, sensor changes --- src/sensors/Encoder.cpp | 17 ++++++++++++ src/sensors/Encoder.h | 5 ++++ src/sensors/HallSensor.cpp | 32 +++++++++++++++++++--- src/sensors/HallSensor.h | 8 ++++-- src/sensors/MagneticSensorAnalog.cpp | 40 +++------------------------- src/sensors/MagneticSensorAnalog.h | 15 +---------- src/sensors/MagneticSensorPWM.cpp | 37 +++---------------------- src/sensors/MagneticSensorPWM.h | 13 +-------- 8 files changed, 65 insertions(+), 102 deletions(-) diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..20506aee 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -102,8 +102,25 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getAngle(){ + return _2PI * (pulse_counter % (int)cpr); +} + +float Encoder::getPosition(){ return _2PI * (pulse_counter) / ((float)cpr); } +double Encoder::getPrecisePosition(){ + return _2PI * (pulse_counter) / ((double)cpr); +} +int32_t Encoder::getFullRotations(){ + return pulse_counter / (int)cpr; +} +float Encoder::getShaftAngle(){ + return getAngle(); +} + + + + /* Shaft velocity calculation function using mixed time and frequency measurement technique diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 7a0f7a40..679bad21 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -60,9 +60,14 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + float getPosition() override; + double getPrecisePosition() override; + int32_t getFullRotations() override; + /** * returns 0 if it does need search for absolute zero * 0 - encoder without index diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..063b3593 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,7 +96,7 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { Shaft angle calculation */ float HallSensor::getAngle() { - return ((electric_rotations * 6 + electric_sector) / cpr) * _2PI ; + return getShaftAngle(); } /* @@ -104,14 +104,39 @@ float HallSensor::getAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0); } } + + +float HallSensor::getShaftAngle() { + return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; +} + + +float HallSensor::getPosition() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; +} + + +double HallSensor::getPrecisePosition() { + return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; +} + + +int32_t HallSensor::getFullRotations() { + return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); +} + + + + + // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ @@ -149,4 +174,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index c836cc5d..28ab1578 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -50,13 +50,17 @@ class HallSensor: public Sensor{ // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups - float cpr;//!< HallSensor cpr number + int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + float getPosition() override; + double getPrecisePosition() override; + int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; @@ -77,7 +81,7 @@ class HallSensor: public Sensor{ Direction decodeDirection(int oldState, int newState); void updateState(); - volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel volatile int C_active; //!< current active states of C channel diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..7c343d63 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -23,47 +23,15 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getAngle(){ +float MagneticSensorAnalog::getShaftAngle(){ // raw data from the sensor - raw_count = getRawCount(); - - int delta = raw_count - raw_count_prev; - // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; -} - -// Shaft velocity calculation -float MagneticSensorAnalog::getVelocity(){ - // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle - return velocity; + raw_count = getRawCount(); + return ( (float) (raw_count) / (float)cpr) * _2PI; } // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 12f78c76..7212ad9a 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,10 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; - + float getShaftAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; @@ -48,16 +45,6 @@ class MagneticSensorAnalog: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //! (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; + return( (float) (raw_count) / (float)cpr) * _2PI; } -// get velocity (rad/s) -float MagneticSensorPWM::getVelocity(){ - return velocity; -} // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 32d6daee..645624d1 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -23,9 +23,7 @@ class MagneticSensorPWM: public Sensor{ int pinPWM; // get current angle (rad) - float getAngle() override; - // get current angular velocity (rad/s) - float getVelocity() override; + float getShaftAngle() override; // pwm handler void handlePWM(); @@ -50,15 +48,6 @@ class MagneticSensorPWM: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //! Date: Mon, 31 May 2021 00:41:38 +0200 Subject: [PATCH 190/749] sensor precision fix - refactored API --- src/common/base_classes/Sensor.cpp | 15 ++++--- src/common/base_classes/Sensor.h | 60 ++++++++++++++++++---------- src/sensors/Encoder.cpp | 13 +++--- src/sensors/Encoder.h | 6 +-- src/sensors/HallSensor.cpp | 21 +++++----- src/sensors/HallSensor.h | 4 +- src/sensors/MagneticSensorAnalog.cpp | 2 +- src/sensors/MagneticSensorAnalog.h | 2 +- src/sensors/MagneticSensorI2C.cpp | 2 +- src/sensors/MagneticSensorI2C.h | 2 +- src/sensors/MagneticSensorPWM.cpp | 2 +- src/sensors/MagneticSensorPWM.h | 2 +- src/sensors/MagneticSensorSPI.cpp | 2 +- src/sensors/MagneticSensorSPI.h | 10 +---- 14 files changed, 81 insertions(+), 62 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 93f4e7fe..4853a8aa 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -3,8 +3,9 @@ #include "../time_utils.h" -float Sensor::getAngle() { - float val = getShaftAngle(); + +float Sensor::updateSensor() { + float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation @@ -14,7 +15,6 @@ float Sensor::getAngle() { } - /** get current angular velocity (rad/s)*/ float Sensor::getVelocity() { // calculate sample time @@ -30,14 +30,19 @@ float Sensor::getVelocity() { } +float Sensor::getShaftAngle() { + return angle_prev; +} + + -float Sensor::getPosition(){ +float Sensor::getAngle(){ return (float)full_rotations * _2PI + angle_prev; } -double Sensor::getPrecisePosition() { +double Sensor::getPreciseAngle() { return (double)full_rotations * (double)_2PI + (double)angle_prev; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 32ba7b25..7f9aea7c 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -32,7 +32,7 @@ enum Pullup{ * method. getAngle() returns a float value, in radians, representing the current shaft angle in the * range 0 to 2*PI (one full turn). * - * To function correctly, the sensor class getAngle() method has to be called sufficiently quickly. Normally, + * To function correctly, the sensor class updateSensor() method has to be called sufficiently quickly. Normally, * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly * enough, both for correct motor and sensor operation. * @@ -43,17 +43,26 @@ enum Pullup{ */ class Sensor{ public: - /** - * Get current shaft angle, as a float in radians, in the range 0 to 2PI. + /** + * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with + * the hardware. + */ + virtual float getShaftAngle(); + + /** + * Get current position (in rad) including full rotations and shaft angle + * Note that this value has limited precision as the number of rotations increases, + * because the limited precision of float can't capture the large angle of the full + * rotations and the small angle of the shaft angle at the same time. */ virtual float getAngle(); + /** - * Get current shaft angle, as a float in radians, in the range 0 to 2PI. - * This method is pure virtual and must be implemented in subclasses. - * Calling this method directly does not update the base-class internal fields. - * Use getAngle() when calling from other code. + * On architectures supporting it, this will return a double precision position value, + * which should have improved precision for large position values. */ - virtual float getShaftAngle()=0; + virtual double getPreciseAngle(); + /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values @@ -62,22 +71,23 @@ class Sensor{ virtual float getVelocity(); /** - * Get current position (in rad) including full rotations and shaft angle - * Note that this value has limited precision as the number of rotations increases, - * as the limited precision of float can't capture the large angle of the full rotations - * and the small angle of the shaft angle at the same time. - */ - virtual float getPosition(); - - /** - * On architectures supporting it, this will return a double precision position value + * Get the number of full rotations */ - virtual double getPrecisePosition(); + virtual int32_t getFullRotations(); /** - * Get the number of full rotations + * Updates the sensor values by reading the hardware sensor. + * Some implementations may work with interrupts, and not need this. + * The base implementation calls getSensorAngle(), and updates internal + * fields for angle, timestamp and full rotations. + * This method must be called frequently enough to guarantee that full + * rotations are not "missed" due to infrequent polling. + * Override in subclasses if alternative behaviours are required for your + * sensor hardware. + * + * Returns the current value of getShaftAngle() */ - virtual int32_t getFullRotations(); + virtual float updateSensor(); /** * returns 0 if it does need search for absolute zero @@ -86,6 +96,16 @@ class Sensor{ */ virtual int needsSearch(); protected: + /** + * Get current shaft angle from the sensor hardware, and + * return it as a float in radians, in the range 0 to 2PI. + * + * This method is pure virtual and must be implemented in subclasses. + * Calling this method directly does not update the base-class internal fields. + * Use updateSensor() when calling from outside code. + */ + virtual float getSensorAngle()=0; + // velocity calculation variables float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 20506aee..fdf1f065 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -101,23 +101,22 @@ void Encoder::handleIndex() { /* Shaft angle calculation */ -float Encoder::getAngle(){ +float Encoder::getSensorAngle(){ + return getShaftAngle(); +} +float Encoder::getShaftAngle(){ return _2PI * (pulse_counter % (int)cpr); } -float Encoder::getPosition(){ +float Encoder::getAngle(){ return _2PI * (pulse_counter) / ((float)cpr); } -double Encoder::getPrecisePosition(){ +double Encoder::getPreciseAngle(){ return _2PI * (pulse_counter) / ((double)cpr); } int32_t Encoder::getFullRotations(){ return pulse_counter / (int)cpr; } -float Encoder::getShaftAngle(){ - return getAngle(); -} - diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 679bad21..c3cab450 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -60,12 +60,12 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getSensorAngle() override; float getShaftAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getPosition() override; - double getPrecisePosition() override; + float getAngle() override; + double getPreciseAngle() override; int32_t getFullRotations() override; /** diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 063b3593..e563e376 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -92,11 +92,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } + + +float HallSensor::getSensorAngle() { + return getShaftAngle(); +} + + + /* Shaft angle calculation */ -float HallSensor::getAngle() { - return getShaftAngle(); +float HallSensor::getShaftAngle() { + return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; } /* @@ -114,17 +122,12 @@ float HallSensor::getVelocity(){ -float HallSensor::getShaftAngle() { - return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; -} - - -float HallSensor::getPosition() { +float HallSensor::getAngle() { return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } -double HallSensor::getPrecisePosition() { +double HallSensor::getPreciseAngle() { return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 28ab1578..ef991b77 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -54,12 +54,12 @@ class HallSensor: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getSensorAngle() override; float getShaftAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getPosition() override; - double getPrecisePosition() override; + double getPreciseAngle() override; int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 7c343d63..04fc89ab 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -28,7 +28,7 @@ void MagneticSensorAnalog::init(){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getShaftAngle(){ +float MagneticSensorAnalog::getSensorAngle(){ // raw data from the sensor raw_count = getRawCount(); return ( (float) (raw_count) / (float)cpr) * _2PI; diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 7212ad9a..6f787b95 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,7 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 7e2bc6c2..dc3cac4c 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -68,7 +68,7 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getShaftAngle(){ +float MagneticSensorI2C::getSensorAngle(){ // (number of full rotations)*2PI + current sensor angle return ( getRawCount() / (float)cpr) * _2PI ; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index f5119643..29bebee4 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -46,7 +46,7 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index a97dc255..8320868f 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -30,7 +30,7 @@ void MagneticSensorPWM::init(){ } // get current angle (rad) -float MagneticSensorPWM::getShaftAngle(){ +float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor raw_count = getRawCount(); return( (float) (raw_count) / (float)cpr) * _2PI; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 645624d1..b42b23d6 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -23,7 +23,7 @@ class MagneticSensorPWM: public Sensor{ int pinPWM; // get current angle (rad) - float getShaftAngle() override; + float getSensorAngle() override; // pwm handler void handlePWM(); diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index e0f72cdb..89155b0a 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -78,7 +78,7 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getShaftAngle(){ +float MagneticSensorSPI::getSensorAngle(){ return (getRawCount() / (float)cpr) * _2PI; } diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index de5dea50..33aad3f3 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -44,7 +44,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getShaftAngle() override; + float getSensorAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; @@ -72,14 +72,6 @@ class MagneticSensorSPI: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //! Date: Mon, 31 May 2021 01:43:32 +0200 Subject: [PATCH 191/749] adapted motor code to new API, added comments for precision issues --- src/BLDCMotor.cpp | 56 ++++++++++++++++++++++-------- src/common/base_classes/Sensor.cpp | 2 +- src/common/base_classes/Sensor.h | 2 +- 3 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..d202aae2 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - shaft_angle = sensor->getAngle(); + shaft_angle = sensor->updateSensor(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -164,14 +164,14 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - float mid_angle = sensor->getAngle(); + float mid_angle = sensor->updateSensor(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0 ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - float end_angle = sensor->getAngle(); + float end_angle = sensor->updateSensor(); setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved @@ -201,7 +201,7 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->updateSensor(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -217,7 +217,8 @@ int BLDCMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroSearch() { - + // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision + // of float is sufficient. if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -229,7 +230,7 @@ int BLDCMotor::absoluteZeroSearch() { angleOpenloop(1.5*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->updateSensor(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -247,15 +248,25 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->updateSensor(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; + // shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; // electrical angle - need shaftAngle to be called first + // TODO sensor precision: this will have precision issues because the shaft_angle does... electrical_angle = electricalAngle(); switch (torque_controller) { @@ -300,14 +311,14 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { - // get angular velocity + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + // get angular velocity - sensor precision: this value is numerically precise. It is filtered by Velocity LPF, and downsamling (depending on sensor) shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; - // downsampling (optional) - if(motion_cnt++ < motion_downsample) return; - motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; @@ -320,11 +331,14 @@ void BLDCMotor::move(float new_target) { current_sp = target; // if current/foc_current torque control break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - // calculate the torque command + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ @@ -335,7 +349,7 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control @@ -348,13 +362,16 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor voltage.d = 0; @@ -572,6 +589,8 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // quick fix for strange cases (micros overflow + timestamp not defined) if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + // sensor precision: this calculation is numerically precise since we re-normalize + // the shaft-angle to the range 0-2PI // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes @@ -581,6 +600,8 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + // sensor precision: this calculation is numerically precise, since shaft_angle is + // in the range 0-2PI // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -603,6 +624,9 @@ float BLDCMotor::angleOpenloop(float target_angle){ // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) + // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point + // where small position changes are no longer captured by the precision of floats + // when the total position is large. if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; shaft_velocity = velocity_limit; @@ -616,6 +640,10 @@ float BLDCMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle + // TODO sensor precision: this calculation is not numerically precise. The angle is not constrained, + // and can grow large. It first gets multiplied by the number of pole-pairs (in _electrocalAngle()), + // and then gets normalized to 0-2PI (before it gets used in setPhaseVoltage()). At large angles, + // the precision can mean the range 0-2PI is not well represented at the end of that calculation. setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 4853a8aa..015dfca9 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -11,7 +11,7 @@ float Sensor::updateSensor() { // if overflow happened track it as full rotation if(abs(d_angle) > (0.8*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; angle_prev = val; - return val; + return getAngle(); } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 7f9aea7c..0f9a5efe 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -85,7 +85,7 @@ class Sensor{ * Override in subclasses if alternative behaviours are required for your * sensor hardware. * - * Returns the current value of getShaftAngle() + * Returns the same value as getAngle() as its result */ virtual float updateSensor(); From a5df2341754023d81a9742c207f54245ad1b6da4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 May 2021 01:53:47 +0200 Subject: [PATCH 192/749] add full rotations to velocity calculations --- src/common/base_classes/Sensor.cpp | 5 +++-- src/common/base_classes/Sensor.h | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 015dfca9..21f6797f 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,7 +2,7 @@ #include "../foc_utils.h" #include "../time_utils.h" - +// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 float Sensor::updateSensor() { float val = getSensorAngle(); @@ -22,9 +22,10 @@ float Sensor::getVelocity() { // quick fix for strange cases (micros overflow) if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; // velocity calculation - float vel = (angle_prev - vel_angle_prev)/Ts; + float vel = ( (full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; // save variables for future pass vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; vel_angle_prev_ts = angle_prev_ts; return vel; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 0f9a5efe..c4c0dc6f 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -112,6 +112,7 @@ class Sensor{ float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity long vel_angle_prev_ts=0; //!< last velocity calculation timestamp int32_t full_rotations=0; // full rotation tracking + int32_t vel_full_rotations=0; }; #endif From c642756349e804ffc889c838c82bdbb6afa6497c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 31 May 2021 22:26:04 +0200 Subject: [PATCH 193/749] comments --- src/common/base_classes/Sensor.h | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index c4c0dc6f..2082230a 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -45,12 +45,15 @@ class Sensor{ public: /** * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with - * the hardware. + * the hardware. Base implementation uses the values returned by updateSensor() so that + * the same values are returned until updateSensor() is called again. */ virtual float getShaftAngle(); /** - * Get current position (in rad) including full rotations and shaft angle + * Get current position (in rad) including full rotations and shaft angle. + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. * Note that this value has limited precision as the number of rotations increases, * because the limited precision of float can't capture the large angle of the full * rotations and the small angle of the shaft angle at the same time. @@ -60,18 +63,23 @@ class Sensor{ /** * On architectures supporting it, this will return a double precision position value, * which should have improved precision for large position values. + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. */ virtual double getPreciseAngle(); /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values - * previously returned by getShaftAngle(). + * returned by updateSensor() so that it only makes sense to call this if updateSensor() + * has been called in the meantime. */ virtual float getVelocity(); /** * Get the number of full rotations + * Base implementation uses the values returned by updateSensor() so that the same + * values are returned until updateSensor() is called again. */ virtual int32_t getFullRotations(); @@ -107,12 +115,12 @@ class Sensor{ virtual float getSensorAngle()=0; // velocity calculation variables - float angle_prev=0; // result of last call to getAngle, used for full rotations and velocity + float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity - long vel_angle_prev_ts=0; //!< last velocity calculation timestamp + long vel_angle_prev_ts=0; // last velocity calculation timestamp int32_t full_rotations=0; // full rotation tracking - int32_t vel_full_rotations=0; + int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation }; #endif From 42ce5511a66fbe078ee7c8a99490ae7304d548e9 Mon Sep 17 00:00:00 2001 From: "Sroka, Kamil" Date: Mon, 31 May 2021 23:19:11 +0200 Subject: [PATCH 194/749] Use float instead of double constants for improved speed --- .../bluepill_position_control.ino | 26 ++-- .../bluepill_position_control.ino | 26 ++-- .../full_control_serial.ino | 20 +-- .../full_control_serial.ino | 20 +-- .../esp32_position_control.ino | 24 ++-- .../esp32_position_control.ino | 20 +-- .../position_control/position_control.ino | 30 ++--- .../nano33IoT_velocity_control.ino | 8 +- .../single_full_control_example.ino | 18 +-- .../double_full_control_example.ino | 26 ++-- .../single_full_control_example.ino | 16 +-- .../double_full_control_example.ino | 30 ++--- .../single_full_control_example.ino | 18 +-- .../encoder/angle_control/angle_control.ino | 40 +++--- .../angle_control/angle_control.ino | 36 +++--- .../angle_control/angle_control.ino | 26 ++-- .../current_control/current_control.ino | 26 ++-- .../velocity_control/velocity_control.ino | 36 +++--- .../hall_sensor/velocity_control.ino | 36 +++--- .../velocity_control/velocity_control.ino | 26 ++-- .../full_control_serial.ino | 16 +-- .../full_control_serial.ino | 18 ++- .../full_control_serial.ino | 17 ++- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 10 +- .../osc_esp32_fullcontrol.ino | 18 +-- .../alignment_and_cogging_test.ino | 12 +- .../find_pole_pairs_number.ino | 46 +++---- .../find_pole_pairs_number.ino | 44 +++---- .../step_dir_listener_software_interrupt.ino | 16 +-- .../step_dir_motor_example.ino | 12 +- .../bldc_driver_6pwm_standalone.ino | 6 +- src/BLDCMotor.cpp | 16 +-- src/StepperMotor.cpp | 66 +++++----- src/common/base_classes/BLDCDriver.h | 22 ++-- src/common/base_classes/Sensor.cpp | 8 +- src/common/defaults.h | 40 +++--- src/common/foc_utils.cpp | 30 ++--- src/common/foc_utils.h | 46 +++---- src/common/pid.cpp | 13 +- src/communication/Commander.h | 120 +++++++++--------- src/current_sense/InlineCurrentSense.cpp | 45 ++++--- src/current_sense/LowsideCurrentSense.cpp | 32 ++--- src/current_sense/hardware_api.h | 12 +- .../hardware_specific/esp32_mcu.cpp | 6 +- .../hardware_specific/generic_mcu.cpp | 26 ++-- .../hardware_specific/samd21_mcu.cpp | 38 +++--- src/drivers/BLDCDriver3PWM.cpp | 14 +- src/drivers/BLDCDriver6PWM.cpp | 22 ++-- src/drivers/StepperDriver2PWM.cpp | 12 +- src/drivers/StepperDriver4PWM.cpp | 10 +- .../hardware_specific/atmega2560_mcu.cpp | 40 +++--- .../hardware_specific/atmega328_mcu.cpp | 42 +++--- src/drivers/hardware_specific/esp32_mcu.cpp | 88 ++++++------- src/drivers/hardware_specific/generic_mcu.cpp | 25 ++-- src/drivers/hardware_specific/rp2040_mcu.cpp | 2 +- src/drivers/hardware_specific/samd_mcu.cpp | 8 +- src/drivers/hardware_specific/stm32_mcu.cpp | 30 ++--- src/drivers/hardware_specific/teensy_mcu.cpp | 26 ++-- src/sensors/Encoder.cpp | 10 +- src/sensors/HallSensor.cpp | 19 ++- src/sensors/MagneticSensorAnalog.cpp | 18 +-- src/sensors/MagneticSensorI2C.cpp | 56 ++++---- src/sensors/MagneticSensorPWM.cpp | 16 +-- src/sensors/MagneticSensorSPI.cpp | 46 +++---- 64 files changed, 859 insertions(+), 868 deletions(-) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 267c00fb..d0768fd2 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with encoder - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -32,10 +32,10 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB, doI); + encoder.enableInterrupts(doA, doB, doI); // link the motor to the sensor motor.linkSensor(&encoder); @@ -54,20 +54,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -75,11 +75,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -97,7 +97,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 0adf4c17..1b47170d 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with magnetic sensor - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -47,37 +47,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -99,7 +99,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -112,7 +112,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 746dde3d..842f383a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -20,7 +20,7 @@ #define INH_C 11 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -45,7 +45,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -76,14 +76,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -111,7 +111,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 3e936460..6c75855a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -23,7 +23,7 @@ #define INL_C 10 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -48,7 +48,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -77,14 +77,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -112,7 +112,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 9864ca0b..3943017b 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -1,4 +1,4 @@ -/** +/** * ESP32 position motion control example with encoder * */ @@ -23,14 +23,14 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -46,20 +46,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -67,11 +67,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -92,7 +92,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -104,7 +104,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 4bd53b55..77cfeeed 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -44,37 +44,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -96,7 +96,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index e9355f65..5b5c8e40 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -1,15 +1,15 @@ /** - * + * * HMBGC position motion control example with encoder - * + * * - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6) * - Encoder is connected to A0 and A1 - * - * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library + * + * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library * - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager - * + * * See docs.simplefoc.com for more info. - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { PciManager.registerListener(&listenerB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -65,20 +65,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -86,11 +86,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -109,7 +109,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -121,7 +121,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index fc46a338..2c9baaef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -17,7 +17,7 @@ BLDCMotor motor = BLDCMotor(7); BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // instantiate the commander Commander command = Commander(SerialUSB); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } @@ -35,11 +35,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 9; //motor.P_angle.P = 20; motor.init(); diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index 16c7755b..0b3075d3 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -22,7 +22,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -38,15 +38,15 @@ void setup() { motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -60,7 +60,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -68,7 +68,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 0; @@ -76,9 +76,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index e9f4e840..c048956b 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7); @@ -30,14 +30,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -55,14 +55,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -78,16 +78,16 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - + // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -99,7 +99,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 5d02ecf2..10e26c9f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -19,7 +19,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -33,15 +33,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -55,11 +55,11 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -69,7 +69,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 757ae0f1..3399d2fa 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7); @@ -39,14 +39,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -64,14 +64,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -87,8 +87,8 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - - + + // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_sense1); @@ -99,12 +99,12 @@ void setup() { // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -116,7 +116,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } @@ -132,4 +132,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index c0f6a10b..67d1d869 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -23,7 +23,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -37,15 +37,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -59,7 +59,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -67,7 +67,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -75,9 +75,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index a8e8b997..942f73dc 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -1,27 +1,27 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -53,10 +53,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -68,7 +68,7 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] @@ -77,11 +77,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -89,9 +89,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -99,11 +99,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -121,7 +121,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -133,7 +133,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 90219b5b..4bd7163f 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -1,20 +1,20 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and hall sensor + * 1) Configure the motor and hall sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > This is for Arduino UNO example code for running angle motion control specifically * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the sensr.enableInterrupts(doA,doB,doC) and avoid using PciManger - * - * + * + * */ #include // software interrupt library @@ -46,10 +46,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -71,11 +71,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -83,9 +83,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -93,11 +93,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -115,7 +115,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -127,7 +127,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index d22289bd..752030c9 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -1,11 +1,11 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * + * */ #include @@ -42,38 +42,38 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -93,7 +93,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -106,7 +106,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index ad75b573..8b1ec96b 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -1,7 +1,7 @@ /** - * + * * Torque control example using current control loop. - * + * */ #include @@ -25,11 +25,11 @@ float target_current = 0; Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_current, cmd); } -void setup() { - +void setup() { + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -46,10 +46,10 @@ void setup() { motor.linkCurrentSense(¤t_sense); // set torque mode: - // TorqueControlType::dc_current + // TorqueControlType::dc_current // TorqueControlType::voltage // TorqueControlType::foc_current - motor.torque_controller = TorqueControlType::foc_current; + motor.torque_controller = TorqueControlType::foc_current; // set motion control loop to be used motor.controller = MotionControlType::torque; @@ -58,17 +58,17 @@ void setup() { motor.PID_current_q.I= 300; motor.PID_current_d.P= 5; motor.PID_current_d.I = 300; - motor.LPF_current_q.Tf = 0.01; - motor.LPF_current_d.Tf = 0.01; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; // foc currnet control parameters (stm/esp/due/teensy) // motor.PID_current_q.P = 5; // motor.PID_current_q.I= 1000; // motor.PID_current_d.P= 5; // motor.PID_current_d.I = 1000; - // motor.LPF_current_q.Tf = 0.002; // 1ms default - // motor.LPF_current_d.Tf = 0.002; // 1ms default + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -91,7 +91,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index a3974b38..f0f1e3e8 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -1,28 +1,28 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -59,7 +59,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -80,11 +80,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -92,11 +92,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -119,7 +119,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -131,7 +131,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 07b70dcd..1d39173a 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -1,20 +1,20 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and sensor + * 1) Configure the motor and sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : - * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { // initialize sensor sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -64,15 +64,15 @@ void setup() { // aligning voltage [V] motor.voltage_sensor_align = 3; - + // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -80,11 +80,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -107,7 +107,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -119,7 +119,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index a16385df..a57fe634 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -1,12 +1,12 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * + * + * * By using the serial terminal set the velocity value you want to motor to obtain * */ @@ -32,7 +32,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { - + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -48,11 +48,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -60,13 +60,13 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering - // default 5ms - try different values to see what is the best. + // default 5ms - try different values to see what is the best. // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -88,7 +88,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -100,7 +100,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 30d18297..186d257e 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using encoder - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -36,7 +36,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -53,15 +53,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -87,7 +87,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 7a202f94..df7b76ac 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using Hall sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -43,7 +43,7 @@ void setup() { // initialize encoder sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -61,15 +61,15 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -95,7 +95,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -112,5 +112,3 @@ void loop() { // user communication command.run(); } - - diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 97216a9e..098f6888 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using magnetic sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -51,15 +51,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the control type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -85,7 +85,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -98,8 +98,7 @@ void loop() { // velocity, position or voltage // if tatget not set in parameter uses motor.target variable motor.move(); - + // user communication command.run(); } - diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index b0a24d25..afb95294 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -93,11 +93,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 8; //motor.P_angle.P = 20; motor.init(); @@ -107,9 +107,9 @@ void setup() { } // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // angle set point variable -float target_angle = 1.0; +float target_angle = 1.0f; void motorControl(OSCMessage &msg){ diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 917f569c..7c6aa4f0 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -30,7 +30,7 @@ WiFiUDP Udp; IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) const unsigned int outPort = 8000; // remote port (not needed for receive) const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) -#define POWER_SUPPLY 7.4 +#define POWER_SUPPLY 7.4f @@ -46,8 +46,8 @@ String m2Prefix("/M2"); // we store seperate set-points for each motor, of course -float set_point1 = 0.0; -float set_point2 = 0.0; +float set_point1 = 0.0f; +float set_point2 = 0.0f; OSCErrorCode error; @@ -135,11 +135,11 @@ void setup() { motor1.linkDriver(&driver1); motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; motor1.controller = MotionControlType::velocity; - motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.P = 0.2f; motor1.PID_velocity.I = 20; - motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.D = 0.001f; motor1.PID_velocity.output_ramp = 1000; - motor1.LPF_velocity.Tf = 0.01; + motor1.LPF_velocity.Tf = 0.01f; motor1.voltage_limit = POWER_SUPPLY; motor1.P_angle.P = 20; motor1.init(); @@ -152,11 +152,11 @@ void setup() { motor2.linkDriver(&driver2); motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; motor2.controller = MotionControlType::velocity; - motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.P = 0.2f; motor2.PID_velocity.I = 20; - motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.D = 0.001f; motor2.PID_velocity.output_ramp = 1000; - motor2.LPF_velocity.Tf = 0.01; + motor2.LPF_velocity.Tf = 0.01f; motor2.voltage_limit = POWER_SUPPLY; motor2.P_angle.P = 20; motor2.init(); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 708e2d9e..ca254a5f 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -9,7 +9,7 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); /** - * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. + * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. * It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle) * This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear * An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal. @@ -28,9 +28,9 @@ void testAlignmentAndCogging(int direction) { float stDevSum = 0; - float mean = 0.0; - float prev_mean = 0.0; - + float mean = 0.0f; + float prev_mean = 0.0f; + for (int i = 0; i < sample_count; i++) { @@ -39,7 +39,7 @@ void testAlignmentAndCogging(int direction) { motor.move(electricAngle * PI / 180); _delay(5); - // measure + // measure float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; @@ -87,7 +87,7 @@ void setup() { motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=motor.voltage_sensor_align; diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index e6a1d20b..e6187921 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -31,7 +31,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void setup() { - + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -57,20 +57,20 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the encoder angle + // read the encoder angle float angle_begin = encoder.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; + motor_angle += 0.01f; motor.move(motor_angle); } _delay(1000); @@ -93,7 +93,7 @@ void setup() { Serial.print(" = "); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set FOC loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +146,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index cc4bd4d0..c2785c55 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -56,20 +56,20 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the sensor angle + // read the sensor angle float angle_begin = sensor.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; + motor_angle += 0.01f; sensor.getAngle(); // keep track of the overflow motor.move(motor_angle); } @@ -93,7 +93,7 @@ void setup() { Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set motion control loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +146,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino index d723313c..b6f0ea5f 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -1,6 +1,6 @@ /** - * A simple example of reading step/dir communication - * - this example uses software interrupts - this code is intended primarily + * A simple example of reading step/dir communication + * - this example uses software interrupts - this code is intended primarily * for Arduino UNO/Mega and similar boards with very limited number of interrupt pins */ @@ -10,11 +10,11 @@ #include -// angle +// angle float received_angle = 0; // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(4, 5, 2.0*_PI/200.0); // receive the angle in radians +StepDirListener step_dir = StepDirListener(4, 5, 2.0f*_PI/200.0); // receive the angle in radians void onStep() { step_dir.handle(); } // If no available hadware interrupt pins use the software interrupt @@ -23,21 +23,21 @@ PciListenerImp listenStep(step_dir.pin_step, onStep); void setup() { Serial.begin(115200); - + // init step and dir pins step_dir.init(); // enable software interrupts PciManager.registerListener(&listenStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling step_dir.getValue(); step_dir.attach(&received_angle); - + Serial.println(F("Step/Dir listenning.")); _delay(1000); } void loop() { - Serial.print(received_angle); + Serial.print(received_angle); Serial.print("\t"); Serial.println(step_dir.getValue()); _delay(500); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index 932edc81..f8978577 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -19,7 +19,7 @@ void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200.0); +StepDirListener step_dir = StepDirListener(A4, A5, 2.0f*_PI/200.0); void onStep() { step_dir.handle(); } void setup() { @@ -48,7 +48,7 @@ void setup() { // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -58,7 +58,7 @@ void setup() { motor.PID_velocity.output_ramp = 1000; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 10; @@ -77,12 +77,12 @@ void setup() { // init step and dir pins step_dir.init(); - // enable interrupts + // enable interrupts step_dir.enableInterrupt(onStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling motor.move(step_dir.getValue()); step_dir.attach(&motor.target); - + Serial.println(F("Motor ready.")); Serial.println(F("Listening to step/dir commands!")); _delay(1000); diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 972a74db..478d3974 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -5,7 +5,7 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { - + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -14,8 +14,8 @@ void setup() { driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply driver.voltage_limit = 12; - // daad_zone [0,1] - default 0.02 - 2% - driver.dead_zone = 0.05; + // daad_zone [0,1] - default 0.02f - 2% + driver.dead_zone = 0.05f; // driver init driver.init(); diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..d9e61aac 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -159,7 +159,7 @@ int BLDCMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -167,7 +167,7 @@ int BLDCMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -188,7 +188,7 @@ int BLDCMotor::alignSensor() { // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -226,7 +226,7 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -568,9 +568,9 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -597,9 +597,9 @@ float BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 183c2c65..ca0716b7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -11,7 +11,7 @@ StepperMotor::StepperMotor(int pp, float _R) // save phase resistance number phase_resistance = _R; - // torque control type is voltage by default + // torque control type is voltage by default // current and foc_current not supported yet torque_controller = TorqueControlType::voltage; } @@ -23,7 +23,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Init")); @@ -37,7 +37,7 @@ void StepperMotor::init() { if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - + // update the controller limits if(_isset(phase_resistance)){ // velocity control loop controls current @@ -52,7 +52,7 @@ void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); - + } @@ -101,14 +101,14 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); - + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } - + return exit_flag; } @@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int StepperMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - + // if unknown natural direction if(!_isset(sensor_direction)){ // check if sensor needs zero search @@ -127,7 +127,7 @@ int StepperMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -135,14 +135,14 @@ int StepperMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); - // determine the direction the sensor moved + // determine the direction the sensor moved if (mid_angle == end_angle) { if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); return 0; // failed calibration @@ -153,10 +153,10 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); sensor_direction = Direction::CW; } - // check pole pair number + // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -166,7 +166,7 @@ int StepperMotor::alignSensor() { // zero electric angle not known if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees + // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); @@ -185,7 +185,7 @@ int StepperMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int StepperMotor::absoluteZeroSearch() { - + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -194,7 +194,7 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -218,15 +218,15 @@ int StepperMotor::absoluteZeroSearch() { void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle + // shaft angle shaft_angle = shaftAngle(); - + // if disabled do nothing - if(!enabled) return; + if(!enabled) return; electrical_angle = electricalAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -239,7 +239,7 @@ void StepperMotor::move(float new_target) { // get angular velocity shaft_velocity = shaftVelocity(); // if disabled do nothing - if(!enabled) return; + if(!enabled) return; // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -249,7 +249,7 @@ void StepperMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance; + else voltage.q = target*phase_resistance; voltage.d = 0; break; case MotionControlType::angle: @@ -259,7 +259,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage + // if torque controlled through voltage // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -270,7 +270,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control + // if torque controlled through voltage control // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -295,12 +295,12 @@ void StepperMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Sine PWM algorithms // - space vector not implemented yet -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park transformation // angle normalization in between 0 and 2pi @@ -323,18 +323,18 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -352,9 +352,9 @@ float StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) @@ -368,12 +368,12 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; - + return Uq; } \ No newline at end of file diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 5764aa30..a657d17c 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,7 +5,7 @@ class BLDCDriver{ public: - + /** Initialise hardware */ virtual int init() = 0; /** Enable hardware */ @@ -14,26 +14,26 @@ class BLDCDriver{ virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage + float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor - + float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - - /** - * Set phase voltages to the harware - * + + /** + * Set phase voltages to the harware + * * @param Ua - phase A voltage * @param Ub - phase B voltage * @param Uc - phase C voltage */ virtual void setPwm(float Ua, float Ub, float Uc) = 0; - /** - * Set phase state, enable/disable - * + /** + * Set phase state, enable/disable + * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) @@ -41,4 +41,4 @@ class BLDCDriver{ virtual void setPhaseState(int sa, int sb, int sc) = 0; }; -#endif \ No newline at end of file +#endif diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..81e80aa2 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,7 +2,7 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) @@ -16,15 +16,15 @@ float Sensor::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..41e7c9ba 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -1,12 +1,12 @@ // default configuration values // change this file to optimal values for your application -#define DEF_POWER_SUPPLY 12.0 //!< default power supply voltage +#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage // velocity PI controller params -#define DEF_PID_VEL_P 0.5 //!< default PID controller P value -#define DEF_PID_VEL_I 10.0 //!< default PID controller I value -#define DEF_PID_VEL_D 0.0 //!< default PID controller D value -#define DEF_PID_VEL_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_VEL_P 0.5f //!< default PID controller P value +#define DEF_PID_VEL_I 10.0f //!< default PID controller I value +#define DEF_PID_VEL_D 0.0f //!< default PID controller D value +#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit // current sensing PID values @@ -14,33 +14,33 @@ // for 16Mhz controllers like Arduino uno and mega #define DEF_PID_CURR_P 2 //!< default PID controller P value #define DEF_PID_CURR_I 100 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value -#define DEF_PID_CURR_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value +#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant -#else +#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant +#else // for stm32, due, teensy, esp32 and similar #define DEF_PID_CURR_P 3 //!< default PID controller P value -#define DEF_PID_CURR_I 300.0 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value +#define DEF_PID_CURR_I 300.0f //!< default PID controller I value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value #define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample -#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable +#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable // angle P params -#define DEF_P_ANGLE_P 20.0 //!< default P controller P value -#define DEF_VEL_LIM 20.0 //!< angle velocity limit default +#define DEF_P_ANGLE_P 20.0f //!< default P controller P value +#define DEF_VEL_LIM 20.0f //!< angle velocity limit default -// index search -#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0 //!< default index search velocity +// index search +#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity // align voltage -#define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt +#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant \ No newline at end of file diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 7653f9be..58ac36e5 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,6 +1,6 @@ #include "foc_utils.h" -// int array instead of float array +// int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 @@ -13,21 +13,21 @@ const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,10 // it has to receive an angle in between 0 and 2PI float _sin(float a){ if(a < _PI_2){ - //return sine_array[(int)(199.0*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873* a)]; // float array optimized - return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized + //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; + //return sine_array[(int)(126.6873f* a)]; // float array optimized + return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized }else if(a < _PI){ - // return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873*a)]; // float array optimized - return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized + // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; + //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized + return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; + //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { - // return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; + //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } @@ -54,7 +54,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { return (shaft_angle * pole_pairs); } -// square root approximation function using +// square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat @@ -67,7 +67,7 @@ float _sqrtApprox(float number) {//low in fat y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 30e99b4f..e58fbaa0 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,31 +12,31 @@ #define _UNUSED(v) (void) (v) // utility defines -#define _2_SQRT3 1.15470053838 -#define _SQRT3 1.73205080757 -#define _1_SQRT3 0.57735026919 -#define _SQRT3_2 0.86602540378 -#define _SQRT2 1.41421356237 -#define _120_D2R 2.09439510239 -#define _PI 3.14159265359 -#define _PI_2 1.57079632679 -#define _PI_3 1.0471975512 -#define _2PI 6.28318530718 -#define _3PI_2 4.71238898038 -#define _PI_6 0.52359877559 +#define _2_SQRT3 1.15470053838f +#define _SQRT3 1.73205080757f +#define _1_SQRT3 0.57735026919f +#define _SQRT3_2 0.86602540378f +#define _SQRT2 1.41421356237f +#define _120_D2R 2.09439510239f +#define _PI 3.14159265359f +#define _PI_2 1.57079632679f +#define _PI_3 1.0471975512f +#define _2PI 6.28318530718f +#define _3PI_2 4.71238898038f +#define _PI_6 0.52359877559f #define NOT_SET -12345.0 #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -// dq current structure +// dq current structure struct DQCurrent_s { float d; float q; }; -// phase current structure +// phase current structure struct PhaseCurrent_s { float a; @@ -54,27 +54,27 @@ struct DQVoltage_s /** * Function approximating the sine calculation by using fixed size array * - execution time ~40us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _sin(float a); /** * Function approximating cosine calculation by using fixed size array * - execution time ~50us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _cos(float a); -/** - * normalizing radian angle to [0,2PI] +/** + * normalizing radian angle to [0,2PI] * @param angle - angle to be normalized - */ + */ float _normalizeAngle(float angle); - -/** - * Electrical angle calculation + +/** + * Electrical angle calculation * * @param shaft_angle - shaft angle of the motor * @param pole_pairs - number of pole pairs @@ -84,7 +84,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs); /** * Function approximating square root function * - using fast inverse square root - * + * * @param value - number */ float _sqrtApprox(float value); diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 5290d814..c887c35b 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -6,9 +6,9 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , D(D) , output_ramp(ramp) // output derivative limit [volts/second] , limit(limit) // output supply limit [volts] - , integral_prev(0.0) - , error_prev(0.0) - , output_prev(0.0) + , integral_prev(0.0f) + , error_prev(0.0f) + , output_prev(0.0f) { timestamp_prev = _micros(); } @@ -17,13 +17,13 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator() (float error){ // calculate the time from the last call unsigned long timestamp_now = _micros(); - float Ts = (timestamp_now - timestamp_prev) * 1e-6; + float Ts = (timestamp_now - timestamp_prev) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations - // proportional part + // proportional part // u_p = P *e(k) float proportional = P * error; // Tustin transform of the integral part @@ -56,4 +56,3 @@ float PIDController::operator() (float error){ timestamp_prev = timestamp_now; return output; } - diff --git a/src/communication/Commander.h b/src/communication/Commander.h index aaa55b11..13777867 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -24,14 +24,14 @@ typedef void (* CommandCallback)(char*); //!< command callback function pointer /** * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`) - * - * - This class can be used in combination with HardwareSerial instance which it would read and write + * + * - This class can be used in combination with HardwareSerial instance which it would read and write * or it can be used to parse strings that have been received from the user outside this library * - Commander class implements command protocol for few standard components of the SimpleFOC library * - FOCMotor * - PIDController * - LowPassFilter - * - Commander also provides a very simple command > callback interface that enables user to + * - Commander also provides a very simple command > callback interface that enables user to * attach a callback function to certain command id - see function add() */ class Commander @@ -40,7 +40,7 @@ class Commander /** * Default constructor receiving a serial interface that it uses to output the values to * Also if the function run() is used it uses this serial instance to read the serial for user commands - * + * * @param serial - Serial com port instance * @param eol - the end of line sentinel character * @param echo - echo last typed character (for command line feedback) @@ -49,45 +49,45 @@ class Commander Commander(char eol = '\n', bool echo = false); /** - * Function reading the serial port and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the serial port and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places - * '?' - Scan command - displays all the labels of attached nodes - */ + * '?' - Scan command - displays all the labels of attached nodes + */ void run(); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param reader - temporary stream to read user input * @param eol - temporary end of line sentinel - */ + */ void run(Stream &reader, char eol = '\n'); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param user_input - string of user inputs - */ + */ void run(char* user_input); /** * Function adding a callback to the coomander withe the command id * @param id - char command letter - * @param onCommand - function pointer void function(char*) + * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent */ void add(char id , CommandCallback onCommand, char* label = nullptr); @@ -101,48 +101,48 @@ class Commander char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) /** - * + * * FOC motor (StepperMotor and BLDCMotor) command interface * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) - * 'D' - D current PID controller & LPF (see function pid and lpf for commands) - * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) - * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) - * 'L' - Limits + * 'D' - D current PID controller & LPF (see function pid and lpf for commands) + * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) + * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) + * 'L' - Limits * sub-commands: - * 'C' - Current - * 'U' - Voltage - * 'V' - Velocity - * 'C' - Motion control type config + * 'C' - Current + * 'U' - Voltage + * 'V' - Velocity + * 'C' - Motion control type config * sub-commands: - * 'D' - downsample motiron loop - * '0' - torque - * '1' - velocity - * '2' - angle - * 'T' - Torque control type + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type * sub-commands: - * '0' - voltage - * '1' - current - * '2' - foc_current - * 'E' - Motor status (enable/disable) + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) * sub-commands: - * '0' - enable - * '1' - disable - * 'R' - Motor resistance - * 'S' - Sensor offsets + * '0' - enable + * '1' - disable + * 'R' - Motor resistance + * 'S' - Sensor offsets * sub-commands: - * 'M' - sensor offset - * 'E' - sensor electrical zero - * 'M' - Monitoring control + * 'M' - sensor offset + * 'E' - sensor electrical zero + * 'M' - Monitoring control * sub-commands: - * 'D' - downsample monitoring - * 'C' - clear monitor - * 'S' - set monitoring variables - * 'G' - get variable value - * '' - Target get/set - * + * 'D' - downsample monitoring + * 'C' - clear monitor + * 'S' - set monitoring variables + * 'G' - get variable value + * '' - Target get/set + * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) - * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5) + * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) * */ void motor(FOCMotor* motor, char* user_cmd); @@ -170,7 +170,7 @@ class Commander * Float variable scalar command interface * - It only has one property - one float value * - It can be get by sending an empty string '\n' - * - It can be set by sending 'value' - (ex. 0.01 for settin *value=0.01) + * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); @@ -184,19 +184,19 @@ class Commander // helping variable for serial communication reading char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline int rec_cnt = 0; //!< number of characters receives - + // serial printing functions /** * print the string message only if verbose mode on * @param message - message to be printed */ - void printVerbose(const char* message); + void printVerbose(const char* message); /** - * Print the string message only if verbose mode on + * Print the string message only if verbose mode on * - Function handling the case for strings defined by F macro * @param message - message to be printed */ - void printVerbose(const __FlashStringHelper *message); + void printVerbose(const __FlashStringHelper *message); /** * print the numbers to the serial with desired decimal point number * @param message - number to be printed diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 879395d1..43e78b9f 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -12,7 +12,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -72,36 +72,36 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; - + // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); - _delay(200); + _delay(200); PhaseCurrent_s c = getPhaseCurrents(); // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); // align phase A float ab_ratio = fabs(c.a / c.b); float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }else if( ab_ratio < 0.7f ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; - pinA = pinB; + pinA = pinB; pinB = tmp_pinA; gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; - pinA = pinC; + pinA = pinC; pinC= tmp_pinA; gain_a *= _sign(c.c); exit_flag = 2;// signal that pins have been switched @@ -109,35 +109,35 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // error in current sense - phase either not measured or bad connection return 0; } - + // set phase B active and phases A and C down driver->setPwm(0, voltage, 0); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); float ba_ratio = fabs(c.b/c.a); float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; - pinB = pinA; + pinB = pinA; pinA = tmp_pinB; gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; - pinB = pinC; + pinB = pinC; pinC = tmp_pinB; gain_b *= _sign(c.c); exit_flag = 2; // signal that pins have been switched @@ -150,7 +150,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ if(_isset(pinC)){ // set phase B active and phases A and C down driver->setPwm(0, 0, voltage); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 50; i++) { @@ -170,4 +170,3 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 4 - success but pins reconfigured and gains inverted return exit_flag; } - diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index e0df940a..383422d6 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -12,7 +12,7 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -41,9 +41,9 @@ void LowsideCurrentSense::calibrateOffsets(){ _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0; - offset_ib = offset_ib / 1000.0; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0; + offset_ia = offset_ia / 1000.0f; + offset_ib = offset_ib / 1000.0f; + if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; } // read all three phase currents (if possible 2 or 3) @@ -86,25 +86,25 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); // align phase A float ab_ratio = fabs(c.a / c.b); float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }else if( ab_ratio < 0.7f ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; pinA = pinB; pinB = tmp_pinA; gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; pinA = pinC; @@ -123,24 +123,24 @@ int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); float ba_ratio = fabs(c.b/c.a); float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; pinB = pinA; pinA = tmp_pinB; gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; pinB = pinC; diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7b648cde..cacd8410 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -6,14 +6,14 @@ /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltageInline(const int pinA); /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C @@ -21,8 +21,8 @@ float _readADCVoltageInline(const int pinA); void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); /** - * function reading an ADC value and returning the read voltage - * + * function reading an ADC value and returning the read voltage + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C @@ -33,7 +33,7 @@ void _startADC3PinConversionLowSide(); /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltageLowSide(const int pinA); @@ -42,4 +42,4 @@ float _readADCVoltageLowSide(const int pinA); * function syncing the Driver with the ADC for the LowSide Sensing */ void _driverSyncLowSide(); -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 3f7e83b3..6d62b2e3 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -6,8 +6,8 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4095.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; //Current readings from internal current sensor amplifiers @@ -31,7 +31,7 @@ float _readADCVoltageLowSide(const int pin){ } void _startADC3PinConversionLowSide(){ - + } // function reading an ADC value and returning the read voltage diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dccd3601..241cdea4 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -3,8 +3,8 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 5.0f + #define _ADC_RESOLUTION 1024.0f #ifndef cbi #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #endif @@ -12,20 +12,20 @@ #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) #endif #elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #elif defined(ESP_H) // or esp32 // do nothing implemented in esp32_mcu.h #elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 3.3f + #define _ADC_RESOLUTION 1024.0f #else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 + #define _ADC_VOLTAGE 5.0f + #define _ADC_RESOLUTION 1024.0f #endif // adc counts to voltage conversion ratio @@ -46,9 +46,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ if( _isset(pinC) ) pinMode(pinC, INPUT); #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) - // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz - // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) + // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz + // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample cbi(ADCSRA, ADPS2); sbi(ADCSRA, ADPS1); sbi(ADCSRA, ADPS0); diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 8bb6d38c..574f7f7c 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -9,8 +9,8 @@ static int _pinA, _pinB, _pinC; static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; /** - * function reading an ADC value and returning the read voltage - * + * function reading an ADC value and returning the read voltage + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C @@ -18,7 +18,7 @@ static SAMDCurrentSenseADCDMA instance; void _configureADCLowSide(const int pinA,const int pinB,const int pinC) { _pinA = pinA; - _pinB = pinB; + _pinB = pinB; _pinC = pinC; freeRunning = true; instance.init(pinA, pinB, pinC); @@ -30,13 +30,13 @@ void _startADC3PinConversionLowSide() } /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltageLowSide(const int pinA) { instance.readResults(a, b, c); - + if(pinA == _pinA) return instance.toVolts(a); if(pinA == _pinB) @@ -72,7 +72,7 @@ static void adcStopWithDMA(void); static void adcStartWithDMA(void); /** - * @brief ADC sync wait + * @brief ADC sync wait * @retval void */ static __inline__ void ADCsync() __attribute__((always_inline, unused)); @@ -82,9 +82,9 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() { - + return &instance; } @@ -136,7 +136,7 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { } void SAMDCurrentSenseADCDMA::initPins(){ - + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -145,7 +145,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); - if( _isset(pinC) ) + if( _isset(pinC) ) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; pinMode(pinC, INPUT); @@ -160,13 +160,13 @@ void SAMDCurrentSenseADCDMA::initPins(){ void SAMDCurrentSenseADCDMA::initADC(){ - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297f V Supply VDDANA ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; @@ -215,7 +215,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ */ ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ADCsync(); ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor ADCsync(); @@ -246,9 +246,9 @@ void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); - - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT; DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts descriptor.descaddr = 0; @@ -289,7 +289,7 @@ void adcStartWithDMA(void){ ADCsync(); ADC->SWTRIG.bit.FLUSH = 1; ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x01; + ADC->CTRLA.bit.ENABLE = 0x01; ADCsync(); } diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 11215863..7ccd09fa 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -39,7 +39,7 @@ void BLDCDriver3PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver3PWM::init() { // PWM pins pinMode(pwmA, OUTPUT); @@ -62,7 +62,7 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin -void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); @@ -73,16 +73,16 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { - + // limit the voltage in driver Ua = _constrain(Ua, 0.0, voltage_limit); Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index d51eec0a..6039f69e 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,9 +15,9 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; - + // dead zone initial - 2% - dead_zone = 0.02; + dead_zone = 0.02f; } @@ -39,9 +39,9 @@ void BLDCDriver6PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver6PWM::init() { - + // PWM pins pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); @@ -55,22 +55,22 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; - // configure 6pwm + // configure 6pwm // hardware specific function - depending on driver and mcu return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); } // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit); - Uc = _constrain(Uc, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); @@ -78,6 +78,6 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 09dd3e16..47a49401 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -27,7 +27,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, dir2a = _dir2; // phase 2 direction pin // these pins are not used dir1b = NOT_SET; - dir2b = NOT_SET; + dir2b = NOT_SET; // enable_pin pin enable_pin1 = en1; @@ -59,7 +59,7 @@ void StepperDriver2PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver2PWM::init() { // PWM pins pinMode(pwm1, OUTPUT); @@ -83,22 +83,22 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { - float duty_cycle1(0.0),duty_cycle2(0.0); +void StepperDriver2PWM::setPwm(float Ua, float Ub) { + float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); - + // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); - + // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); } \ No newline at end of file diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f752ea6e..cfcb7aef 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -37,7 +37,7 @@ void StepperDriver4PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver4PWM::init() { // PWM pins @@ -59,17 +59,17 @@ int StepperDriver4PWM::init() { // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - else + else duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - + if( Ubeta > 0 ) duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); else diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 5236fb03..3d50dc45 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -15,13 +15,13 @@ void _pinHighFrequency(const int pin){ TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 10 || pin == 9 ) TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 5 || pin == 3 || pin == 2) + else if (pin == 5 || pin == 3 || pin == 2) TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 8 || pin == 7 || pin == 6) TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 44 || pin == 45 || pin == 46) TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 - + } @@ -46,23 +46,23 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins @@ -74,25 +74,25 @@ void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const i _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } // function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm // supports Arudino/ATmega2560 int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure complementary pwm on low side @@ -121,7 +121,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz @@ -132,12 +132,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -148,7 +148,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index be8dadb1..2c64ee64 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ @@ -12,9 +12,9 @@ void _pinHighFrequency(const int pin){ } if (pin == 9 || pin == 10 ) TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) + if (pin == 3 || pin == 11) TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - + } // function setting the high pwm frequency to the supplied pins @@ -42,47 +42,47 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -117,7 +117,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { _UNUSED(pwm_frequency); _UNUSED(dead_zone); @@ -130,12 +130,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -146,7 +146,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 0bc2d863..c87e3b70 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -6,13 +6,13 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -// empty motor slot +// empty motor slot #define _EMPTY_SLOT -20 #define _TAKEN_SLOT -21 // ABI bus frequency - would be better to take it from somewhere // but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6 +#define _MCPWM_FREQ 160e6f // preferred pwm resolution default #define _PWM_RES_DEF 2048 @@ -70,7 +70,7 @@ typedef struct { } bldc_6pwm_motor_slots_t; // define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A @@ -78,19 +78,19 @@ bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { }; // define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; + }; // define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; + }; // define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A @@ -109,12 +109,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - + if (_isset(dead_zone)){ - // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); } _delay(100); @@ -128,21 +128,21 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); + prescaler = _constrain(prescaler, 0, 128); // now calculate the real resolution timer period necessary (pwm resolution) // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler mcpwm_num->timer[0].period.prescale = prescaler; mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; _delay(1); //set period mcpwm_num->timer[0].period.period = resolution_corrected; @@ -152,13 +152,13 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].period.upmethod = 0; mcpwm_num->timer[1].period.upmethod = 0; mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - // _delay(1); + _delay(1); + // _delay(1); //restart the timers mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); + _delay(1); // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined // in the default Espressif MCPWM headers. The correct const may be used // when https://github.com/espressif/esp-idf/issues/5429 is resolved. @@ -174,15 +174,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_2pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -191,8 +191,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { break; } } - - // disable all the slots with the same MCPWM + + // disable all the slots with the same MCPWM // disable 3pwm bldc motor which would go in the same slot esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -219,15 +219,15 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max bldc_3pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -236,7 +236,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM // disable 2pwm steppr motor which would go in the same slot esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -266,10 +266,10 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -278,7 +278,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -297,7 +297,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); @@ -314,7 +314,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found // se the PWM on the slot timers @@ -331,7 +331,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found // se the PWM on the slot timers @@ -349,7 +349,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found // se the PWM on the slot timers @@ -367,12 +367,12 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -384,7 +384,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // if no slots available if(slot_num >= 2) return -1; - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -397,7 +397,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); @@ -409,7 +409,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return + // return return 0; } @@ -417,7 +417,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 6549de07..a2c98aac 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -53,34 +53,34 @@ __attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, co } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -100,4 +100,3 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(pinC_l); return; } - diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 097b7a6d..7be8faaf 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -147,7 +147,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in inline float swDti(float val, float dt) { float ret = dt+val; - if (ret>1.0) ret = 1.0; + if (ret>1.0) ret = 1.0f; return ret; } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 1b035dfd..e1d38a80 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -704,7 +704,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -715,7 +715,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinB_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -726,7 +726,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinC_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -866,5 +866,3 @@ void printTCCConfiguration(tccConfiguration& info) { #endif #endif - - diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 3f213711..b0ac00e7 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -27,12 +27,12 @@ void _setPwm(int ulPin, uint32_t value, int resolution) } -// init pin pwm +// init pin pwm HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) { PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); @@ -61,7 +61,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; @@ -129,17 +129,17 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in PinName wlPinName = digitalPinToPinName(pinC_l); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); @@ -148,20 +148,20 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); // dead time is set in nanoseconds - uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); HT->pause(); HT->refresh(); - HT->resume(); + HT->resume(); return HT; } // returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns 1 all the channels belong to the same timer - hardware inverted channels // returns -1 if neither - avoid configuring - error!!! int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ PinName nameAH = digitalPinToPinName(pinA_h); @@ -177,7 +177,7 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else @@ -230,7 +230,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); } @@ -315,11 +315,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); break; } diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 5f70db4f..67bb0ad1 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +8,7 @@ // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); - analogWriteFrequency(pin, freq); + analogWriteFrequency(pin, freq); } @@ -42,25 +42,25 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); - _setHighFrequency(pwm_frequency, pinD); + _setHighFrequency(pwm_frequency, pinD); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware @@ -68,10 +68,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..4d110d7d 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -112,12 +112,12 @@ float Encoder::getVelocity(){ // timestamp long timestamp_us = _micros(); // sampling time calculation - float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6; + float Th = (timestamp_us - pulse_timestamp) * 1e-6f; long dN = pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) @@ -129,8 +129,8 @@ float Encoder::getVelocity(){ float dt = Ts + prev_Th - Th; pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; - // if more than 0.05 passed in between impulses - if ( Th > 0.1) pulse_per_second = 0; + // if more than 0.05f passed in between impulses + if ( Th > 0.1f) pulse_per_second = 0; // velocity calculation float velocity = pulse_per_second / ((float)cpr) * (_2PI); diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..a7a9f005 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -7,14 +7,14 @@ - pp - pole pairs */ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ - + // hardware pins pinA = _hallA; pinB = _hallB; pinC = _hallC; // hall has 6 segments per electrical revolution - cpr = _pp * 6; + cpr = _pp * 6; // extern pullup as default pullup = Pullup::USE_EXTERN; @@ -40,12 +40,12 @@ void HallSensor::handleC() { /** * Updates the state and sector following an interrupt - */ + */ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); - + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed if (new_hall_state == hall_state) { return; @@ -74,7 +74,7 @@ void HallSensor::updateState() { } else { pulse_diff = 0; } - + pulse_timestamp = new_pulse_timestamp; total_interrupts++; old_direction = direction; @@ -87,7 +87,7 @@ void HallSensor::updateState() { * ... // for debug or call driver directly? * } * sensor.attachSectorCallback(onSectorChange); - */ + */ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } @@ -107,12 +107,12 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0f); } } -// HallSensor initialisation of the hardware pins +// HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ // initialise the electrical rotations to 0 @@ -134,7 +134,7 @@ void HallSensor::init(){ B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); - + pulse_timestamp = _micros(); } @@ -149,4 +149,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..f67d432b 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -6,7 +6,7 @@ * @param _max_raw_count the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096) */ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){ - + pinAnalog = _pinAnalog; cpr = _max_raw_count - _min_raw_count; @@ -26,30 +26,30 @@ void MagneticSensorAnalog::init(){ // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorAnalog::getAngle(){ // raw data from the sensor - raw_count = getRawCount(); + raw_count = getRawCount(); int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - // calculate velocity here + // calculate velocity here long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; + float Ts = ( now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index c9029f6e..56d202dc 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -19,21 +19,21 @@ MagneticSensorI2CConfig_s AS5048_I2C = { // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address -// @param _bit_resolution bit resolution of the sensor +// @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ // chip I2C address - chip_address = _chip_address; + chip_address = _chip_address; // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) cpr = pow(2, _bit_resolution); - + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -43,13 +43,13 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, } MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; + chip_address = config.chip_address; // angle read register of the magnetic sensor angle_register_msb = config.angle_register; // register maximum value (counts per revolution) cpr = pow(2, config.bit_resolution); - + int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; // extraction masks @@ -61,36 +61,36 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; - + // I2C communication begin wire->begin(); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; } @@ -98,15 +98,15 @@ float MagneticSensorI2C::getAngle(){ float MagneticSensorI2C::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; @@ -119,7 +119,7 @@ int MagneticSensorI2C::getRawCount(){ return (int)MagneticSensorI2C::read(angle_register_msb); } -// I2C functions +// I2C functions /* * Read a register from the sensor * Takes the address of the register as a uint8_t @@ -133,14 +133,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { wire->beginTransmission(chip_address); wire->write(angle_reg_msb); wire->endTransmission(false); - + // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - // depending on the sensor architecture there are different combinations of + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB @@ -151,7 +151,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { /* * Checks whether other devices have locked the bus. Can clear SDA locks. -* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda * e.g some stm32 boards with AS5600 chips * Takes the sda_pin and scl_pin * Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW @@ -160,7 +160,7 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { pinMode(scl_pin, INPUT_PULLUP); pinMode(sda_pin, INPUT_PULLUP); - delay(250); + delay(250); if (digitalRead(scl_pin) == LOW) { // Someone else has claimed master!"); @@ -179,15 +179,15 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { } pinMode(sda_pin, INPUT); delayMicroseconds(20); - if (digitalRead(sda_pin) == LOW) { + if (digitalRead(sda_pin) == LOW) { // SDA still blocked - return 2; - } + return 2; + } _delay(1000); } // SDA is clear (HIGH) pinMode(sda_pin, INPUT); pinMode(scl_pin, INPUT); - - return 0; + + return 0; } diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index da4ebf89..a067eb09 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -13,7 +13,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m cpr = _max_raw_count - _min_raw_count; min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - + // define if the sensor uses interrupts is_interrupt_based = false; @@ -23,7 +23,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m void MagneticSensorPWM::init(){ - + // initial hardware pinMode(pinPWM, INPUT); @@ -36,23 +36,23 @@ void MagneticSensorPWM::init(){ raw_count_prev = getRawCount(); } -// get current angle (rad) +// get current angle (rad) float MagneticSensorPWM::getAngle(){ // raw data from sensor raw_count = getRawCount(); - + int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; // calculate velocity here long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; + float Ts = (now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass @@ -80,7 +80,7 @@ int MagneticSensorPWM::getRawCount(){ void MagneticSensorPWM::handlePWM() { // unsigned long now_us = ticks(); unsigned long now_us = _micros(); - + // if falling edge, calculate the pulse length if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 5ee9ecf9..ce9a13cb 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -8,7 +8,7 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x3FFF, - .data_start_bit = 13, + .data_start_bit = 13, .command_rw_bit = 14, .command_parity_bit = 15 }; @@ -22,19 +22,19 @@ MagneticSensorSPIConfig_s MA730_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x0000, - .data_start_bit = 15, + .data_start_bit = 15, .command_rw_bit = 0, // not required .command_parity_bit = 0 // parity not implemented }; // MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register) -// cs - SPI chip select pin +// cs - SPI chip select pin // _bit_resolution sensor resolution bit number // _angle_register - (optional) angle read register - default 0x3FFF MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ - - chip_select_pin = cs; + + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -42,14 +42,14 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; - + command_parity_bit = 15; // for backwards compatibilty command_rw_bit = 14; // for backwards compatibilty data_start_bit = 13; // for backwards compatibilty } MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ - chip_select_pin = cs; + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -57,7 +57,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; - + command_parity_bit = config.command_parity_bit; // for backwards compatibilty command_rw_bit = config.command_rw_bit; // for backwards compatibilty data_start_bit = config.data_start_bit; // for backwards compatibilty @@ -72,7 +72,7 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //setup pins pinMode(chip_select_pin, OUTPUT); - + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); // do any architectures need to set the clock divider for SPI? Why was this in the code? @@ -81,31 +81,31 @@ void MagneticSensorSPI::init(SPIClass* _spi){ digitalWrite(chip_select_pin, HIGH); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorSPI::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; } @@ -113,9 +113,9 @@ float MagneticSensorSPI::getAngle(){ float MagneticSensorSPI::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); @@ -134,7 +134,7 @@ int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); } -// SPI functions +// SPI functions /** * Utility function used to calculate even parity of word */ @@ -174,13 +174,13 @@ word MagneticSensorSPI::read(word angle_register){ digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - + #if defined( ESP_H ) // if ESP32 board delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif - + //Now read the response digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); @@ -188,7 +188,7 @@ word MagneticSensorSPI::read(word angle_register){ //SPI - end transaction spi->endTransaction(); - + register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word const static word data_mask = 0xFFFF >> (16 - bit_resolution); From 709a558062bf3c8670561150f6e8e7799df03787 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 3 Jun 2021 23:51:34 +0200 Subject: [PATCH 195/749] add a (optional) init method to sensor base class --- src/common/base_classes/Sensor.cpp | 15 ++++++++++++++- src/common/base_classes/Sensor.h | 7 +++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 21f6797f..3bb6c734 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -30,6 +30,19 @@ float Sensor::getVelocity() { return vel; } +void Sensor::init() { + // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero) + getSensorAngle(); // call once + delayMicroseconds(1); + vel_angle_prev = getSensorAngle(); // call again + vel_angle_prev_ts = _micros(); + delay(1); + getSensorAngle(); // call once + delayMicroseconds(1); + angle_prev = getSensorAngle(); // call again + angle_prev_ts = _micros(); +} + float Sensor::getShaftAngle() { return angle_prev; @@ -57,4 +70,4 @@ int32_t Sensor::getFullRotations() { int Sensor::needsSearch() { return 0; // default false -} \ No newline at end of file +} diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 2082230a..78193158 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -113,6 +113,13 @@ class Sensor{ * Use updateSensor() when calling from outside code. */ virtual float getSensorAngle()=0; + /** + * Call Sensor::init() from your sensor subclass's init method if you want smoother startup + * The base class init() method calls getSensorAngle() several times to initialize the internal fields + * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls + * to sensor.getAngle() and sensor.getVelocity() + */ + virtual void init(); // velocity calculation variables float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity From ce75219c1cd849d4ffae0dc32db8ce1fe266cc80 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 08:14:25 +0200 Subject: [PATCH 196/749] FEAT esp32 adc wrapper --- keywords.txt | 1 + src/SimpleFOC.h | 1 + src/current_sense/LowsideCurrentSense.cpp | 18 +- .../hardware_specific/atmega_mcu.cpp | 55 ++++ .../hardware_specific/due_mcu.cpp | 38 +++ .../hardware_specific/esp32_adc_driver.cpp | 240 ++++++++++++++++++ .../hardware_specific/esp32_adc_driver.h | 104 ++++++++ .../hardware_specific/esp32_mcu.cpp | 41 ++- .../hardware_specific/generic_mcu.cpp | 64 ++--- .../hardware_specific/stm32_mcu.cpp | 39 +++ .../hardware_specific/teensy_mcu.cpp | 37 +++ 11 files changed, 581 insertions(+), 57 deletions(-) create mode 100644 src/current_sense/hardware_specific/atmega_mcu.cpp create mode 100644 src/current_sense/hardware_specific/due_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.h create mode 100644 src/current_sense/hardware_specific/stm32_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy_mcu.cpp diff --git a/keywords.txt b/keywords.txt index 77152a36..8201c144 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ StepperDriver KEYWORD1 PIDController KEYWORD1 LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 +LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..02e215b9 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowSideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index abebf03d..cad7fdbb 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -27,22 +27,24 @@ void LowsideCurrentSense::init(){ calibrateOffsets(); } // Function finding zero offsets of the ADC -void LowsideCurrentSense::calibrateOffsets(){ +void LowsideCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + // find adc offset = zero current voltage - offset_ia =0; - offset_ib= 0; - offset_ic= 0; + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) - for (int i = 0; i < 1000; i++) { + for (int i = 0; i < calibration_rounds; i++) { offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0; - offset_ib = offset_ib / 1000.0; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0; + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } // read all three phase currents (if possible 2 or 3) diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp new file mode 100644 index 00000000..021da52f --- /dev/null +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -0,0 +1,55 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) + +#define _ADC_VOLTAGE 5.0 +#define _ADC_RESOLUTION 1024.0 + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +#ifndef cbi + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) + // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz + // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample + cbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp new file mode 100644 index 00000000..c6207c05 --- /dev/null +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -0,0 +1,38 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp new file mode 100644 index 00000000..7a6ded1c --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -0,0 +1,240 @@ +// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at + +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if defined(ESP_H) + +#include "esp32_adc_driver.h" +#include "Arduino.h" + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "rom/ets_sys.h" +#include "esp_attr.h" +#include "esp_intr.h" +#include "soc/rtc_io_reg.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" + +static uint8_t __analogAttenuation = 3;//11db +static uint8_t __analogWidth = 3;//12 bits +static uint8_t __analogCycles = 8; +static uint8_t __analogSamples = 0;//1 sample +static uint8_t __analogClockDiv = 1; + +// Width of returned answer () +static uint8_t __analogReturnedWidth = 12; + +void __analogSetWidth(uint8_t bits){ + if(bits < 9){ + bits = 9; + } else if(bits > 12){ + bits = 12; + } + __analogReturnedWidth = bits; + __analogWidth = bits - 9; + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); + + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); +} + +void __analogSetCycles(uint8_t cycles){ + __analogCycles = cycles; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); +} + +void __analogSetSamples(uint8_t samples){ + if(!samples){ + return; + } + __analogSamples = samples - 1; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); +} + +void __analogSetClockDiv(uint8_t clockDiv){ + if(!clockDiv){ + return; + } + __analogClockDiv = clockDiv; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); +} + +void __analogSetAttenuation(uint8_t attenuation) +{ + __analogAttenuation = attenuation & 3; + uint32_t att_data = 0; + int i = 10; + while(i--){ + att_data |= __analogAttenuation << (i * 2); + } + WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels + WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); +} + +void IRAM_ATTR __analogInit(){ + static bool initialized = false; + if(initialized){ + return; + } + + __analogSetAttenuation(__analogAttenuation); + __analogSetCycles(__analogCycles); + __analogSetSamples(__analogSamples + 1);//in samples + __analogSetClockDiv(__analogClockDiv); + __analogSetWidth(__analogWidth + 9);//in bits + + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); + + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== + + initialized = true; +} + +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0 || attenuation > 3){ + return ; + } + __analogInit(); + if(channel > 7){ + SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); + } else { + SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + } +} + +bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + int8_t pad = digitalPinToTouchChannel(pin); + if(pad >= 0){ + uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); + if(touch & (1 << pad)){ + touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) + | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) + | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); + WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); + } + } else if(pin == 25){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 + } else if(pin == 26){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 + } + + pinMode(pin, ANALOG); + + __analogInit(); + return true; +} + +bool IRAM_ATTR __adcStart(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + } + return true; +} + +bool IRAM_ATTR __adcBusy(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 7){ + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + } + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); +} + +uint16_t IRAM_ATTR __adcEnd(uint8_t pin) +{ + + uint16_t value = 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return 0;//not adc pin + } + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); +} + +void __analogReadResolution(uint8_t bits) +{ + if(!bits || bits > 16){ + return; + } + __analogSetWidth(bits); // hadware from 9 to 12 + __analogReturnedWidth = bits; // software from 1 to 16 +} + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ + if(!__adcAttachPin(pin) || !__adcStart(pin)){ + return 0; + } + return __adcEnd(pin); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h new file mode 100644 index 00000000..aff9348b --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -0,0 +1,104 @@ +/* + Arduino.h - Main include file for the Arduino SDK + Copyright (c) 2005-2013 Arduino Team. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#if !defined(MAIN_ESP32_HAL_ADC_DRIVER_H_) && defined(ESP_H) +#define MAIN_ESP32_HAL_ADC_DRIVER_H_ + +#include "esp32-hal.h" + + +/* + * Get ADC value for pin + * */ +uint16_t adcRead(uint8_t pin); + +/* + * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). + * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. + * Range is 1 - 16 + * + * Note: compatibility with Arduino SAM + */ +void __analogReadResolution(uint8_t bits); + +/* + * Sets the sample bits and read resolution + * Default is 12bit (0 - 4095) + * Range is 9 - 12 + * */ +void __analogSetWidth(uint8_t bits); + +/* + * Set number of cycles per sample + * Default is 8 and seems to do well + * Range is 1 - 255 + * */ +void __analogSetCycles(uint8_t cycles); + +/* + * Set number of samples in the range. + * Default is 1 + * Range is 1 - 255 + * This setting splits the range into + * "samples" pieces, which could look + * like the sensitivity has been multiplied + * that many times + * */ +void __analogSetSamples(uint8_t samples); + +/* + * Set the divider for the ADC clock. + * Default is 1 + * Range is 1 - 255 + * */ +void __analogSetClockDiv(uint8_t clockDiv); + +/* + * Set the attenuation for all channels + * Default is 11db + * */ +void __analogSetAttenuation(uint8_t attenuation); + +/* + * Set the attenuation for particular pin + * Default is 11db + * */ +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); + +/* + * Attach pin to ADC (will also clear any other analog mode that could be on) + * */ +bool __adcAttachPin(uint8_t pin); + +/* + * Start ADC conversion on attached pin's bus + * */ +bool __adcStart(uint8_t pin); + +/* + * Check if conversion on the pin's ADC bus is currently running + * */ +bool __adcBusy(uint8_t pin); + +/* + * Get the result of the conversion (will wait if it have not finished) + * */ +uint16_t __adcEnd(uint8_t pin); + +#endif /* MAIN_ESP32_HAL_ADC_H_ */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 7f84f9a9..39d18dbf 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -6,6 +6,11 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" +#include +#include + +#include "esp32_adc_driver.h" + #define _ADC_VOLTAGE 3.3 #define _ADC_RESOLUTION 4095.0 @@ -30,7 +35,6 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } - // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; @@ -43,6 +47,10 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ } void _driverSyncLowSide(){ + // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + + MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt @@ -51,30 +59,51 @@ void _driverSyncLowSide(){ // Read currents when interrupt is triggered static void IRAM_ATTR isr_handler(void*){ +// uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; +// uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; //uint32_t mcpwm_intr_status_2 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + // digitalWrite(14,HIGH); if(mcpwm_intr_status_0 > 0 && currentState == 1){ - a1 = analogRead(_pinA); - //a2 = analogRead(_pinB); + a1 = adcRead(_pinA); + // a2 = analogRead(_pinA); currentState = 2; } else if(mcpwm_intr_status_1 > 0 && currentState == 2){ - a2 = analogRead(_pinB); - //a3 = analogRead(_pinC); + a2 = adcRead(_pinB); + // a3 = analogRead(_pinB); currentState = 1; } /* else if(mcpwm_intr_status_2 > 0 && currentState == 3){ - a3 = analogRead(_pinC); + a3 = adcRead(_pinC); //a1 = analogRead(_pinA); currentState = 1; }*/ + // digitalWrite(14,LOW); + // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = adcRead(pinA); + // uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + #endif diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dccd3601..c5bbcc10 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,56 +1,34 @@ #include "../hardware_api.h" - - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 - #ifndef cbi - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) - #endif - #ifndef sbi - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - #endif -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(ESP_H) // or esp32 - // do nothing implemented in esp32_mcu.h - -#elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 -#endif - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +// if the mcu doen't have defiend analogRead +// __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + return raw_adc * 5.0/1024; } +// function reading an ADC value and returning the read voltage +__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} + +// Configure low side for generic mcu +// cannot do much but +__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); +} + - #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) - // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz - // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample - cbi(ADCSRA, ADPS2); - sbi(ADCSRA, ADPS1); - sbi(ADCSRA, ADPS0); - #endif -} \ No newline at end of file +// sync driver and the adc +__attribute__((weak)) void _driverSyncLowSide(){ } diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..82d66647 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,39 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..dc6048e2 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file From a8acae3aad726f4c821bd1957d68f6d1b66eef07 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 08:53:55 +0200 Subject: [PATCH 197/749] merge of esp32 low side wrapper with current dev --- src/SimpleFOC.h | 2 +- src/current_sense/LowsideCurrentSense.cpp | 2 + src/current_sense/hardware_api.h | 3 + .../hardware_specific/generic_mcu.cpp | 2 + .../hardware_specific/samd21_mcu.cpp | 308 ++++++++++++++++++ .../hardware_specific/samd21_mcu.h | 67 ++++ src/drivers/hardware_specific/samd21_mcu.cpp | 34 +- src/drivers/hardware_specific/samd51_mcu.cpp | 20 +- src/drivers/hardware_specific/samd_mcu.cpp | 162 ++++----- src/drivers/hardware_specific/samd_mcu.h | 5 +- src/sensors/MagneticSensorSPI.cpp | 19 +- 11 files changed, 499 insertions(+), 125 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd21_mcu.cpp create mode 100644 src/current_sense/hardware_specific/samd21_mcu.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 02e215b9..4f9f1108 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,7 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" -#include "current_sense/LowSideCurrentSense.h" +#include "current_sense/LowsideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index cad7fdbb..88213b2e 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -36,6 +36,7 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { + _startADC3PinConversionLowSide(); offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); @@ -50,6 +51,7 @@ void LowsideCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; + _startADC3PinConversionLowSide(); current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 4c4c1086..7b648cde 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,6 +28,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); + +void _startADC3PinConversionLowSide(); + /** * function reading an ADC value and returning the read voltage * diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index c5bbcc10..894af62c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -32,3 +32,5 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } + +__attribute__((weak)) void _startADC3PinConversionLowSide(){ } \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..8bb6d38c --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,308 @@ +#ifdef _SAMD21_ + +#include "samd21_mcu.h" +#include "../hardware_api.h" + + +static bool freeRunning = false; +static int _pinA, _pinB, _pinC; +static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode +static SAMDCurrentSenseADCDMA instance; +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC) +{ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} +void _startADC3PinConversionLowSide() +{ + instance.startADCScan(); +} +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA) +{ + instance.readResults(a, b, c); + + if(pinA == _pinA) + return instance.toVolts(a); + if(pinA == _pinB) + return instance.toVolts(b); + if(pinA == _pinC) + return instance.toVolts(c); + + return NAN; +} + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide() +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); + instance.startADCScan(); + //TODO: hook with PWM interrupts +} + + + + + + + + + + + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +{ + + return &instance; +} + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +{ +} + +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +{ + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; + this->pinAREF = pinAREF; + this->channelDMA = channelDMA; + this->voltageAREF = voltageAREF; + this->maxCountsADC = 1 << adcBits; + countsToVolts = ( voltageAREF / maxCountsADC ); + + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() +} + + +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); +} + +bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; + if(_isset(pinC)) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[ainC]; + } + return true; +} + + +float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { + return counts * countsToVolts; +} + +void SAMDCurrentSenseADCDMA::initPins(){ + + pinMode(pinAREF, INPUT); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); + if( _isset(pinC) ) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + pinMode(pinC, INPUT); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); + } + + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; + +} + +void SAMDCurrentSenseADCDMA::initADC(){ + + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X + // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + ADCsync(); // ref 31.6.16 + + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); + +void SAMDCurrentSenseADCDMA::initDMA() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + + +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + + +int iii = 0; + +void adcStopWithDMA(void){ + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x00; + // ADCsync(); + // if(iii++ % 1000 == 0) + // { + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); + // } + + +} + +void adcStartWithDMA(void){ + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); + ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); +} + +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adcStopWithDMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..c0cec74a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,67 @@ +#ifdef _SAMD21_ + +#ifndef CURRENT_SENSE_SAMD21_H +#define CURRENT_SENSE_SAMD21_H + +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif + +#include + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + + +class SAMDCurrentSenseADCDMA +{ + +public: + static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); + SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); + void startADCScan(); + bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + float toVolts(uint16_t counts); +private: + + void adcToDMATransfer(void *rxdata, uint32_t hwords); + + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel + bool freeRunning; + + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); + +}; + +#endif + + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..08201b97 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..1b035dfd 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b3d82dee..5ee9ecf9 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -75,11 +75,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif + // do any architectures need to set the clock divider for SPI? Why was this in the code? + //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); // velocity calculation init @@ -170,35 +167,27 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); #if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word From ae31910086b0ea1f53684f329b206b8025f647d7 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 09:06:14 +0200 Subject: [PATCH 198/749] added weak definitions for - esp32 redefine analogRead --- src/current_sense/hardware_specific/generic_mcu.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 894af62c..86c5c716 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,7 +1,9 @@ #include "../hardware_api.h" // if the mcu doen't have defiend analogRead -// __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; +#if defined(ESP_H) + __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; +#endif // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ @@ -32,5 +34,4 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } - __attribute__((weak)) void _startADC3PinConversionLowSide(){ } \ No newline at end of file From 644389bad3a9bed6d114a9455681578173c651ff Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 14:50:26 +0200 Subject: [PATCH 199/749] FEAT fix of esp32 driver + added support for per phase lpf for current sensing --- keywords.txt | 3 ++ src/common/defaults.h | 5 ++- src/current_sense/InlineCurrentSense.cpp | 6 +-- src/current_sense/InlineCurrentSense.h | 9 ++++- src/current_sense/LowsideCurrentSense.cpp | 6 +-- src/current_sense/LowsideCurrentSense.h | 7 ++++ src/current_sense/hardware_api.h | 7 ++-- .../hardware_specific/esp32_adc_driver.cpp | 19 +--------- .../hardware_specific/esp32_adc_driver.h | 28 +++----------- .../hardware_specific/esp32_mcu.cpp | 9 +++-- .../hardware_specific/generic_mcu.cpp | 5 --- .../hardware_specific/samd_mcu.cpp | 37 +++++++++++++++++++ src/drivers/hardware_specific/generic_mcu.cpp | 5 +++ 13 files changed, 87 insertions(+), 59 deletions(-) create mode 100644 src/current_sense/hardware_specific/samd_mcu.cpp diff --git a/keywords.txt b/keywords.txt index 8201c144..1d8156f5 100644 --- a/keywords.txt +++ b/keywords.txt @@ -49,6 +49,9 @@ LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 LPF_angle KEYWORD2 +lpf_a KEYWORD2 +lpf_b KEYWORD2 +lpf_c KEYWORD2 MotionControlType KEYWORD1 TorqueControlType KEYWORD1 diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..63357fc5 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,4 +43,7 @@ // align voltage #define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant + +// current sense default parameters +#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 879395d1..454975dc 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps + current.a = lpf_a(_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps + current.b = lpf_b(_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : lpf_c(_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index da46d104..2aaca4a4 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -4,7 +4,9 @@ #include "Arduino.h" #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -33,8 +35,13 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - private: + // per phase low pass fileters + LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: + // hardware variables int pinA; //!< pin A analog pin for current measurement int pinB; //!< pin B analog pin for current measurement diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 88213b2e..d643a056 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -52,9 +52,9 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps + current.b = lpf_b(_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : lpf_b(_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 2300d5f6..38c6d1a2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -4,8 +4,10 @@ #include "Arduino.h" #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,6 +36,11 @@ class LowsideCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain + // per phase low pass fileters + LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: // hardware variables diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7b648cde..126f4342 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,9 +28,6 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); - -void _startADC3PinConversionLowSide(); - /** * function reading an ADC value and returning the read voltage * @@ -42,4 +39,8 @@ float _readADCVoltageLowSide(const int pinA); * function syncing the Driver with the ADC for the LowSide Sensing */ void _driverSyncLowSide(); + + +void _startADC3PinConversionLowSide(); + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 7a6ded1c..01e1ca60 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -1,21 +1,6 @@ -// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at - -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#if defined(ESP_H) - #include "esp32_adc_driver.h" -#include "Arduino.h" + +#ifdef ESP_H #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h index aff9348b..a01eef19 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -1,28 +1,11 @@ -/* - Arduino.h - Main include file for the Arduino SDK - Copyright (c) 2005-2013 Arduino Team. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - */ -#if !defined(MAIN_ESP32_HAL_ADC_DRIVER_H_) && defined(ESP_H) -#define MAIN_ESP32_HAL_ADC_DRIVER_H_ -#include "esp32-hal.h" +#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#include "Arduino.h" +#ifdef ESP_H /* * Get ADC value for pin * */ @@ -101,4 +84,5 @@ bool __adcBusy(uint8_t pin); * */ uint16_t __adcEnd(uint8_t pin); -#endif /* MAIN_ESP32_HAL_ADC_H_ */ \ No newline at end of file +#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ +#endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 39d18dbf..c880c56a 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) +#ifdef ESP_H #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" @@ -11,8 +11,8 @@ #include "esp32_adc_driver.h" -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4095.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; //Current readings from internal current sensor amplifiers @@ -98,8 +98,9 @@ float _readADCVoltageInline(const int pinA){ // uint32_t raw_adc = analogRead(pinA); return raw_adc * _ADC_CONV; } + // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void _configureADCInline(const int pinA,const int pinB, const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 86c5c716..acae178c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,10 +1,5 @@ #include "../hardware_api.h" -// if the mcu doen't have defiend analogRead -#if defined(ESP_H) - __attribute__((weak)) int analogRead(uint8_t pin){ return 0;}; -#endif - // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..0c953ea1 --- /dev/null +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) + +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 1024.0 + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 6549de07..7f39f4a3 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,5 +1,10 @@ #include "../hardware_api.h" +// if the mcu doen't have defiend analogWrite +#if defined(ESP_H) + __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; +#endif + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic From dc51797290c952361240f364f38ce89ebe2de997 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:17:02 +0200 Subject: [PATCH 200/749] FEAT fix merge bugs --- src/current_sense/LowsideCurrentSense.cpp | 7 ++++--- .../hardware_specific/atmega_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/due_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/esp32_mcu.cpp | 7 ------- .../hardware_specific/generic_mcu.cpp | 2 +- .../hardware_specific/samd21_mcu.cpp | 15 ++++++++------- src/current_sense/hardware_specific/samd_mcu.cpp | 4 ++-- src/current_sense/hardware_specific/stm32_mcu.cpp | 4 ++-- .../hardware_specific/teensy_mcu.cpp | 4 ++-- 9 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 346d32cf..086edbc9 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -43,12 +43,13 @@ void LowsideCurrentSense::calibrateOffsets(){ _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0f; - offset_ib = offset_ib / 1000.0f; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } // read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 021da52f..75265c03 100644 --- a/src/current_sense/hardware_specific/atmega_mcu.cpp +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) -#define _ADC_VOLTAGE 5.0 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 5.0f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp index c6207c05..ef450b35 100644 --- a/src/current_sense/hardware_specific/due_mcu.cpp +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__arm__) && defined(__SAM3X8E__) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 5f5adff4..c880c56a 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -35,13 +35,6 @@ float _readADCVoltageLowSide(const int pin){ return raw_adc * _ADC_CONV; } -<<<<<<< HEAD -void _startADC3PinConversionLowSide(){ - -} - -======= ->>>>>>> dev // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index ca8c7d4e..ecc0b79b 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -3,7 +3,7 @@ // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * 5.0/1024; + return raw_adc * 5.0f/1024.0f; } // function reading an ADC value and returning the read voltage diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f6f481f6..af2c0048 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -19,23 +19,24 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) { _pinA = pinA; _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} void _startADC3PinConversionLowSide() { instance.startADCScan(); } /** * function reading an ADC value and returning the read voltage -<<<<<<< HEAD - * -======= * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ float _readADCVoltageLowSide(const int pinA) +{ instance.readResults(a, b, c); -<<<<<<< HEAD - -======= ->>>>>>> dev if(pinA == _pinA) return instance.toVolts(a); if(pinA == _pinB) diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp index 0c953ea1..550d7bee 100644 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -2,8 +2,8 @@ #if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp index 82d66647..19833e11 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -3,8 +3,8 @@ #if defined(_STM32_DEF_) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp index dc6048e2..606ce618 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -2,8 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 1024.0 +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f // adc counts to voltage conversion ratio // some optimizing for faster execution From 2bda51f7422f3c1660ac880b1b2ce9afef3d36fa Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:27:45 +0200 Subject: [PATCH 201/749] merge conflict hardware_api --- src/current_sense/hardware_api.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 126f4342..49f2e7b3 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -28,6 +28,9 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) * @param pinC - adc pin C */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); + +void _startADC3PinConversionLowSide(); + /** * function reading an ADC value and returning the read voltage * From 1a6269d6e1e7c7aaa29699fcffe8569c7f475e48 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 15:30:15 +0200 Subject: [PATCH 202/749] preparation for 2.1.2 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index af8559f1..0d89185d 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.1 +version=2.1.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 7382429330a9a3d1cbe07d4dcf3171f03fac65fc Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 5 Jun 2021 16:07:38 +0200 Subject: [PATCH 203/749] FEAT due to the low memory of the Arduino UNO remove the per phase filtering for now --- src/current_sense/InlineCurrentSense.cpp | 6 +++--- src/current_sense/InlineCurrentSense.h | 8 ++++---- src/current_sense/LowsideCurrentSense.cpp | 6 +++--- src/current_sense/LowsideCurrentSense.h | 8 ++++---- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index eb6493ab..43e78b9f 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = lpf_a(_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps - current.b = lpf_b(_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : lpf_c(_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 2aaca4a4..615c4733 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -35,10 +35,10 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - // per phase low pass fileters - LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter private: diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 086edbc9..18aefd49 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -52,9 +52,9 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = lpf_b(_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : lpf_b(_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 38c6d1a2..9618b572 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -36,10 +36,10 @@ class LowsideCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - // per phase low pass fileters - LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter private: From b860f4d3c4542b09f35247c5bcfc89da449bf181 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 09:21:44 +0200 Subject: [PATCH 204/749] pwm sensor allowed arduino uno attach interrupt --- src/sensors/MagneticSensorPWM.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index a067eb09..713bcc2a 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -95,8 +95,6 @@ void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){ // declare it's interrupt based is_interrupt_based = true; - #if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega168__) && !defined(__AVR_ATmega328PB__) && !defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 - // enable interrupts on pwm input pin - attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); - #endif + // enable interrupts on pwm input pin + attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); } \ No newline at end of file From d6b33f4c6c6ae94937eadc0a62c4cd3c1755b436 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 09:22:28 +0200 Subject: [PATCH 205/749] FIX set voltage d to 0 when in torque mode --- src/BLDCMotor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d9e61aac..93fcee17 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -313,11 +313,13 @@ void BLDCMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage) // if voltage torque control + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; else voltage.q = target*phase_resistance; - else + voltage.d = 0; + }else{ current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: // angle set point From a22c2bcd6fc0025229eae5ac5bd3a847663faae3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 10:11:22 +0200 Subject: [PATCH 206/749] API change stepperdriver2pwm --- .../smartstepper_control.ino | 61 +++++++++++++++++++ .../stepper_driver_2pwm_standalone.ino | 6 +- src/drivers/StepperDriver2PWM.cpp | 14 ++--- src/drivers/StepperDriver2PWM.h | 8 +-- 4 files changed, 75 insertions(+), 14 deletions(-) create mode 100644 examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino new file mode 100644 index 00000000..80f2fe64 --- /dev/null +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -0,0 +1,61 @@ +/** + * Smart Stepper support with SimpleFOClibrary + */ +#include + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, A2); + +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +int in1[2] = {5, 6}; +int in2[2] = {A4, 7}; +StepperDriver2PWM driver = StepperDriver2PWM(4, in1, 9, in2); + +// instantiate the commander +Commander command = Commander(SerialUSB); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // power supply voltage + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // comment out if not needed + motor.useMonitoring(SerialUSB); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "my motor"); + + SerialUSB.println(F("Motor ready.")); + SerialUSB.println(F("Set the target voltage using Serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + motor.loopFOC(); + + // Motion control function + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 7289ac79..4627efa9 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -3,8 +3,10 @@ // Stepper driver instance -// StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) -StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); +// StepperDriver2PWM(pwm1, in1, pwm2, in2, (en1, en2 optional)) +int in1[] = {4,5}; +int in2[] = {9,8}; +StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); // StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional)) // StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 47a49401..a9432b1d 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -1,13 +1,13 @@ #include "StepperDriver2PWM.h" -StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2){ +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){ // Pin initialization pwm1 = _pwm1; // phase 1 pwm pin number - dir1a = _in1a; // phase 1 INA pin number - dir1b = _in1b; // phase 1 INB pin number + dir1a = _in1[0]; // phase 1 INA pin number + dir1b = _in1[1]; // phase 1 INB pin number pwm2 = _pwm2; // phase 2 pwm pin number - dir2a = _in2a; // phase 2 INA pin number - dir2b = _in2b; // phase 2 INB pin number + dir2a = _in2[0]; // phase 2 INA pin number + dir2b = _in2[1]; // phase 2 INB pin number // enable_pin pin enable_pin1 = en1; @@ -94,10 +94,10 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); - if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW ); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); - if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); + if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index cab796d3..b349af06 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -16,15 +16,13 @@ class StepperDriver2PWM: public StepperDriver /** StepperMotor class constructor @param pwm1 PWM1 phase pwm pin - @param in1a IN1A phase dir pin - @param in1b IN1B phase dir pin + @param in1 IN1A phase dir pin @param pwm2 PWM2 phase pwm pin - @param in2a IN2A phase dir pin - @param in2b IN2B phase dir pin + @param in2 IN2A phase dir @param en1 enable pin phase 1 (optional input) @param en2 enable pin phase 2 (optional input) */ - StepperDriver2PWM(int pwm1, int in1a, int in1b, int pwm2, int in2a, int in2b, int en1 = NOT_SET, int en2 = NOT_SET); + StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET); /** StepperMotor class constructor From 98c10e6b96be818b4780025cb2ab75a651098c96 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 6 Jun 2021 10:19:43 +0200 Subject: [PATCH 207/749] avoid compiling samd examples --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 2377cd61..9eea4f03 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control From 22d003c12135795d857d23645f2febe643d4fad3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Jun 2021 22:17:30 +0200 Subject: [PATCH 208/749] performance optimisation - don't return angle on updateSensor() --- src/BLDCMotor.cpp | 12 ++++++++---- src/common/base_classes/Sensor.cpp | 3 +-- src/common/base_classes/Sensor.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c0cb5b32..dda3d745 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - shaft_angle = sensor->updateSensor(); + sensor->updateSensor(); + shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -164,14 +165,16 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - float mid_angle = sensor->updateSensor(); + sensor->updateSensor(); + float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - float end_angle = sensor->updateSensor(); + sensor->updateSensor(); + float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved @@ -201,7 +204,8 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->updateSensor(), pole_pairs)); + sensor->updateSensor(); + zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index a3921998..1f6b649e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -4,14 +4,13 @@ // TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 -float Sensor::updateSensor() { +void Sensor::updateSensor() { float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; angle_prev = val; - return getAngle(); } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 78193158..47929cf3 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -95,7 +95,7 @@ class Sensor{ * * Returns the same value as getAngle() as its result */ - virtual float updateSensor(); + virtual void updateSensor(); /** * returns 0 if it does need search for absolute zero From 9cf18c27415cfe0d43da7ec56a8c2675c1e21196 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 21:09:00 +0200 Subject: [PATCH 209/749] doh! TCC0 has 6 output channels - fixed WO_associations table --- src/drivers/hardware_specific/samd51_mcu.cpp | 40 ++++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 08201b97..0fd339f2 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -31,8 +31,8 @@ struct wo_association WO_associations[] = { { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTB, 11, TC5_CH1, 1, TCC0_CH1, 5, TCC1_CH1, 1}, //? + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, //? { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, @@ -40,34 +40,34 @@ struct wo_association WO_associations[] = { { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, //? - { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, //? + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, //? + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, //? { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH1, 5, TCC1_CH1, 1}, //? - { PORTA, 12, TC2_CH0, 0, TCC0_CH2, 6, TCC1_CH2, 2}, - { PORTA, 13, TC2_CH1, 1, TCC0_CH3, 7, TCC1_CH3, 3}, + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, + { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? - { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH0, 4}, - { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH1, 5}, - { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH2, 6}, - { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH3, 7}, + { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, + { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, + { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, + { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH1, 7}, { PORTC, 16, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTC, 17, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTC, 18, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 { PORTC, 19, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, - { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, - { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH2, 6, NOT_ON_TIMER, 0}, - { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH3, 7, NOT_ON_TIMER, 0}, + { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, + { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH0, 6, NOT_ON_TIMER, 0}, + { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH1, 7, NOT_ON_TIMER, 0}, { PORTD, 20, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, { PORTD, 21, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 4}, - { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 5}, + { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH4, 4}, + { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH5, 5}, { PORTB, 18, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTB, 19, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTB, 20, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 @@ -89,8 +89,8 @@ struct wo_association WO_associations[] = { // PC24-PC28, PA27, RESET -> no TC/TCC peripherals { PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, { PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 6}, - { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 7}, + { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH0, 6}, + { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH1, 7}, // PC30, PC31 -> no TC/TCC peripherals { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, From a1149e187a55323b04d0af2712afd30b35b3deb1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 21:10:29 +0200 Subject: [PATCH 210/749] add enable_active_high setting to 3-PWM driver also --- src/drivers/BLDCDriver3PWM.cpp | 14 +++++++------- src/drivers/BLDCDriver3PWM.h | 1 + 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 7ccd09fa..a0f96599 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,9 +20,9 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, HIGH); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, HIGH); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, HIGH); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high); // set zero to PWM setPwm(0,0,0); } @@ -33,9 +33,9 @@ void BLDCDriver3PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, LOW); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, LOW); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, LOW); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high); } @@ -87,4 +87,4 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); -} \ No newline at end of file +} diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index cea4463e..d6a83590 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,6 +38,7 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number + bool enable_active_high = true; /** * Set phase voltages to the harware From 17dc7fb8135684f7a3deafbbafc62abff8819092 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 7 Jun 2021 22:36:01 +0200 Subject: [PATCH 211/749] comments updated --- src/drivers/hardware_specific/samd51_mcu.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 0fd339f2..b59a43fc 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -31,8 +31,8 @@ struct wo_association WO_associations[] = { { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? - { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, @@ -40,18 +40,18 @@ struct wo_association WO_associations[] = { { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, //? - { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, //? + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, //? - { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, //? + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, - { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? - { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? + { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, + { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, From a875e409a48563429165cd3882d0cd328ff03eed Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 10:50:34 +0200 Subject: [PATCH 212/749] esp32 2 or 3 adc lines automatic --- src/current_sense/LowsideCurrentSense.cpp | 2 + .../hardware_specific/esp32_mcu.cpp | 99 +++++++++++-------- 2 files changed, 59 insertions(+), 42 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 18aefd49..2ff2ce8b 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -23,6 +23,8 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int void LowsideCurrentSense::init(){ // configure ADC variables _configureADCLowSide(pinA,pinB,pinC); + // sync the driver + _driverSyncLowSide(); // calibrate zero offsets calibrateOffsets(); } diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index c880c56a..d4ac0587 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../drivers/hardware_api.h" #ifdef ESP_H @@ -14,15 +15,39 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = adcRead(pinA); + // uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} + +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB, const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +/** + * Low side adc reading implementation +*/ static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; -int a1, a2, a3; //Current readings from internal current sensor amplifiers +int a1, a2, a3; // Current readings from internal current sensor amplifiers int _pinA, _pinB, _pinC; static void IRAM_ATTR isr_handler(void*); byte currentState = 1; -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin){ @@ -39,71 +64,61 @@ float _readADCVoltageLowSide(const int pin){ void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; _pinB = pinB; - if( _isset(pinC) ) _pinC = pinC; + _pinC = pinC; pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - } void _driverSyncLowSide(){ + // high side registers enable interrupt // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt - + // low-side register enable interrupt MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt + if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler } // Read currents when interrupt is triggered static void IRAM_ATTR isr_handler(void*){ -// uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; -// uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; + // // high side + // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; + // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; + // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; + + // low side uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; - //uint32_t mcpwm_intr_status_2 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st; + uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0; - // digitalWrite(14,HIGH); - if(mcpwm_intr_status_0 > 0 && currentState == 1){ - a1 = adcRead(_pinA); - // a2 = analogRead(_pinA); + switch (currentState) + { + case 1 : + if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); currentState = 2; - } - else if(mcpwm_intr_status_1 > 0 && currentState == 2){ - a2 = adcRead(_pinB); - // a3 = analogRead(_pinB); + break; + case 2 : + if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); + currentState = _isset(_pinC) ? 3 : 1; + break; + case 3 : + if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); currentState = 1; + break; } - /* - else if(mcpwm_intr_status_2 > 0 && currentState == 3){ - a3 = adcRead(_pinC); - //a1 = analogRead(_pinA); - currentState = 1; - }*/ - // digitalWrite(14,LOW); + // high side // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; + // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; + // low side MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; - //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; -} - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = adcRead(pinA); - // uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} - -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB, const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; } From 0aa327d46d6ef979e1d4c10058b5b5bbb33797fc Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:40:52 +0200 Subject: [PATCH 213/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 9eea4f03..b3116706 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -4,11 +4,35 @@ jobs: build: name: Test compiling runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno + - esp32:esp32:featheresp32:FlashFreq=80 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + + include: + - arduino-boards-fqbn: arduino:avr:uno + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + + - arduino-boards-fqbn: TM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + + - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false steps: - name: Checkout uses: actions/checkout@master - name: Compile all examples uses: ArminJo/arduino-test-compile@v1.0.0 with: - libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: PciManager + platform-url: ${{ matrix.platform-url }} + sketches-exclude: ${{ matrix.sketches-exclude }} From b966b5dc803c06df55f3c05983ffaa05eff13fc2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:41:48 +0200 Subject: [PATCH 214/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index b3116706..27e5e149 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -16,7 +16,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: TM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, From 9d8bd1027d18db772b19b9ad44471110fa16c8e6 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:46:34 +0200 Subject: [PATCH 215/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 27e5e149..5a48d128 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -33,6 +33,6 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: PciManager + libraries: PciManager platform-url: ${{ matrix.platform-url }} sketches-exclude: ${{ matrix.sketches-exclude }} From 265c4d1042017052ff560a8c4ffacbf9d276d98a Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 11:48:31 +0200 Subject: [PATCH 216/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5a48d128..56f595ed 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,20 +9,20 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - - esp32:esp32:featheresp32:FlashFreq=80 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + #- esp32:esp32:featheresp32:FlashFreq=80 + #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, - - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + # - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + # platform-url: https://dl.espressif.com/dl/package_esp32_index.json + # sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # Do not cancel all jobs / architectures if one job fails fail-fast: false From fecf077333893e1b4da3fd1fddb0b58009517914 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:00:08 +0200 Subject: [PATCH 217/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 56f595ed..25223570 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -30,9 +30,9 @@ jobs: - name: Checkout uses: actions/checkout@master - name: Compile all examples - uses: ArminJo/arduino-test-compile@v1.0.0 + uses: ArminJo/arduino-test-compile@master with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - libraries: PciManager + required-libraries: PciManager platform-url: ${{ matrix.platform-url }} sketches-exclude: ${{ matrix.sketches-exclude }} From 075925d15370d78666d2b753cd6275652cb81826 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:09:19 +0200 Subject: [PATCH 218/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 25223570..6ed2829d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,13 +9,19 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - #- esp32:esp32:featheresp32:FlashFreq=80 + - arduono:sam:due +# - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno + sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + - arduino-boards-fqbn: arduino:sam:uno + sketch-names: single_full_control_example.ino + + # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, @@ -35,4 +41,5 @@ jobs: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} required-libraries: PciManager platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} sketches-exclude: ${{ matrix.sketches-exclude }} From 695ba4622d85b716018cff763f302a8c80d04b11 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:10:22 +0200 Subject: [PATCH 219/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6ed2829d..db9dfc03 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -18,7 +18,7 @@ jobs: sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:uno + - arduino-boards-fqbn: arduino:sam:due sketch-names: single_full_control_example.ino From 1a437171db83a813f27820586291c4af5acdd864 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:24:02 +0200 Subject: [PATCH 220/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index db9dfc03..f294fa7e 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -9,7 +9,7 @@ jobs: matrix: arduino-boards-fqbn: - arduino:avr:uno - - arduono:sam:due + - arduino:sam:arduino_due_x # - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 @@ -18,7 +18,7 @@ jobs: sketch-names: '**.ino' sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:due + - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino From 221e48cfbaa5d73957a767aa8e4ea1e389d9394e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:27:44 +0200 Subject: [PATCH 221/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index f294fa7e..e31384e3 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,25 +10,27 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x -# - esp32:esp32:featheresp32:FlashFreq=80 + - esp32:esp32:featheresp32:FlashFreq=80 #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno sketch-names: '**.ino' + required-libraries: PciManager sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino - + + - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, - # - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - # platform-url: https://dl.espressif.com/dl/package_esp32_index.json - # sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + # Do not cancel all jobs / architectures if one job fails fail-fast: false @@ -39,7 +41,7 @@ jobs: uses: ArminJo/arduino-test-compile@master with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: PciManager + required-libraries: ${{ matrix.required-libraries }} platform-url: ${{ matrix.platform-url }} sketch-names: ${{ matrix.sketch-names }} sketches-exclude: ${{ matrix.sketches-exclude }} From 0bb5256467abb041985c1f3a7da5f762733ab406 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:28:09 +0200 Subject: [PATCH 222/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index e31384e3..25bdb812 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -23,8 +23,8 @@ jobs: sketch-names: single_full_control_example.ino - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control, esp32_i2c_dual_bus_example # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json From d157c86711614ba93d6c3f81a6bed540533dbdea Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:33:03 +0200 Subject: [PATCH 223/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 25bdb812..69131460 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,7 +11,7 @@ jobs: - arduino:avr:uno - arduino:sam:arduino_due_x - esp32:esp32:featheresp32:FlashFreq=80 - #- STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 include: - arduino-boards-fqbn: arduino:avr:uno @@ -22,13 +22,13 @@ jobs: - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control, esp32_i2c_dual_bus_example + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - # - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - # platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - # sketch-names: bluepill_position_control, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino From 2ea35dd491850b1da83a29d69fd46f3d0bf38c11 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:39:37 +0200 Subject: [PATCH 224/749] FIX esp32 error --- .../encoder/esp32_position_control/esp32_position_control.ino | 3 --- .../esp32_position_control/esp32_position_control.ino | 3 --- 2 files changed, 6 deletions(-) diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 3943017b..f6dac940 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -85,9 +85,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 77cfeeed..d66b5e16 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -88,9 +88,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function From 07f842e24d6d7c3b17941cf8e1f15402d531b624 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:41:10 +0200 Subject: [PATCH 225/749] FIX bluepill examples --- .../bluepill_position_control/bluepill_position_control.ino | 2 +- .../bluepill_position_control/bluepill_position_control.ino | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index d0768fd2..be853f0e 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -89,7 +89,7 @@ void setup() { command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); - Serial.println(F(("Set the target angle using serial terminal:")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 1b47170d..e27f1e7a 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -91,9 +91,6 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function From 02616843024aafdf58a195bf80ad3a1c5cce5fad Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:41:51 +0200 Subject: [PATCH 226/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 69131460..8e4f665b 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,8 +10,9 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x - - esp32:esp32:featheresp32:FlashFreq=80 + - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 +# - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE include: - arduino-boards-fqbn: arduino:avr:uno @@ -30,6 +31,10 @@ jobs: platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino + # Do not cancel all jobs / architectures if one job fails From 999d000d532845e1ca1d93e027bf58a32f4058c1 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:46:32 +0200 Subject: [PATCH 227/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 8e4f665b..e04aa980 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,9 +10,10 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:sam:arduino_due_x + - arduino:samd:nano_33_iot - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 -# - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE include: - arduino-boards-fqbn: arduino:avr:uno @@ -22,6 +23,9 @@ jobs: - arduino-boards-fqbn: arduino:sam:arduino_due_x sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:samd:nano_33_iot + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json @@ -31,9 +35,9 @@ jobs: platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino + sketch-names: single_full_control_example.ino From 5c4c07586f7a4ecc1dbcd420ad709967de3d3549 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 12:50:38 +0200 Subject: [PATCH 228/749] FIX nano33 example --- .../nano33IoT_velocity_control/nano33IoT_velocity_control.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index 2c9baaef..70ec28b0 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -6,7 +6,6 @@ #include "Arduino.h" #include #include -#include // this is for an AS5048B absolute magnetic encoder on I2C address 0x41 MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8); From 954545b748d35fff9eaf0e4611106074c9546c47 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 12:52:13 +0200 Subject: [PATCH 229/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index e04aa980..d14dfaab 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -14,6 +14,7 @@ jobs: - esp32:esp32:esp32doit-devkit-v1 - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - arduino:mbed_rp2040:pico include: - arduino-boards-fqbn: arduino:avr:uno @@ -26,6 +27,9 @@ jobs: - arduino-boards-fqbn: arduino:samd:nano_33_iot sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico + sketch-names: open_loop_position_example.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json From dde0ef31431cb552413f8b7e574043c925e51ac2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:16:31 +0200 Subject: [PATCH 230/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index d14dfaab..c24ce6f0 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -15,6 +15,7 @@ jobs: - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - arduino:mbed_rp2040:pico + - adafruit:samd:adafruit_metro_m4 include: - arduino-boards-fqbn: arduino:avr:uno @@ -30,6 +31,10 @@ jobs: - arduino-boards-fqbn: arduino:mbed_rp2040:pico sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + sketch-names: single_full_control_example.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 platform-url: https://dl.espressif.com/dl/package_esp32_index.json From c89ae6bffea9e723f40a49cb7f81e4c710a25b35 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:19:12 +0200 Subject: [PATCH 231/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c24ce6f0..c3d269c3 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -8,43 +8,43 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:avr:uno - - arduino:sam:arduino_due_x - - arduino:samd:nano_33_iot - - esp32:esp32:esp32doit-devkit-v1 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - - arduino:mbed_rp2040:pico - - adafruit:samd:adafruit_metro_m4 + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + - esp32:esp32:esp32doit-devkit-v1 # esm32 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - arduino:mbed_rp2040:pico # rpi pico include: - - arduino-boards-fqbn: arduino:avr:uno + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control - - arduino-boards-fqbn: arduino:sam:arduino_due_x + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: arduino:samd:nano_33_iot + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - - arduino-boards-fqbn: arduino:mbed_rp2040:pico + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example sketch-names: open_loop_position_example.ino - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json sketch-names: single_full_control_example.ino - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://dl.espressif.com/dl/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, magnetic_sensor_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json sketch-names: single_full_control_example.ino From 88d383b6eae0eb9fd6446fa15d91750384beb9ac Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 10 Jun 2021 13:29:19 +0200 Subject: [PATCH 232/749] FEAT added low-side esp32 BRV8302 example --- .../esp32_current_control_low_side.ino | 164 ++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino new file mode 100644 index 00000000..202aa337 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -0,0 +1,164 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A 21 +#define INH_B 19 +#define INH_C 18 + +#define EN_GATE 5 +#define M_PWM 25 +#define M_OC 26 +#define OC_ADJ 12 +#define OC_GAIN 14 + +#define IOUTA 34 +#define IOUTB 35 +#define IOUTC 32 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(22, 23, 500); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.pwm_frequency = 15000; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 1000; + + // initialise motor + motor.init(); + + cs.init(); + cs.driverSync(&driver); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file From 6267e797b0185ea4ddebe23670a68ef33559d432 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:31:12 +0200 Subject: [PATCH 233/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c3d269c3..b8eedae1 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -38,7 +38,7 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json From 5f64df651c37f43b7fc60df6991937cc77956e37 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 10 Jun 2021 13:42:54 +0200 Subject: [PATCH 234/749] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index b8eedae1..07bb15b1 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,7 +21,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 1b11bd422ead5b149aa6635c0976cf025ef69991 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Thu, 1 Jul 2021 16:47:33 +0200 Subject: [PATCH 235/749] Prevent unintended float-to-double casts --- src/BLDCMotor.cpp | 6 +++--- src/common/foc_utils.cpp | 4 ++-- src/common/foc_utils.h | 4 ++-- src/common/pid.cpp | 2 +- src/current_sense/InlineCurrentSense.cpp | 14 +++++++------- src/current_sense/InlineCurrentSense.h | 12 ++++++------ src/current_sense/LowsideCurrentSense.h | 12 ++++++------ src/drivers/BLDCDriver3PWM.cpp | 6 +++--- src/drivers/StepperDriver2PWM.cpp | 4 ++-- src/drivers/StepperDriver4PWM.cpp | 8 ++++---- src/drivers/hardware_specific/stm32_mcu.cpp | 2 +- 11 files changed, 37 insertions(+), 37 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 93fcee17..db713edf 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -458,8 +458,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { center = driver->voltage_limit/2; // Clarke transform Ua = Ualpha + center; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + center; + Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; + Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); @@ -502,7 +502,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (modulation_centered) { diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 58ac36e5..11a1810f 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -23,11 +23,11 @@ float _sin(float a){ }else if(a < _3PI_2){ // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873f*a)]; // int array optimized + return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873f*a)]; // int array optimized + return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e58fbaa0..75bce025 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -5,7 +5,7 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) -#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) @@ -25,7 +25,7 @@ #define _3PI_2 4.71238898038f #define _PI_6 0.52359877559f -#define NOT_SET -12345.0 +#define NOT_SET -12345.0f #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 diff --git a/src/common/pid.cpp b/src/common/pid.cpp index c887c35b..6472c0f4 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -28,7 +28,7 @@ float PIDController::operator() (float error){ float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) - float integral = integral_prev + I*Ts*0.5*(error + error_prev); + float integral = integral_prev + I*Ts*0.5f*(error + error_prev); // antiwindup - limit the output integral = _constrain(integral, -limit, limit); // Discrete derivation diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 43e78b9f..bc3bc4c6 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -80,9 +80,9 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -117,9 +117,9 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -155,7 +155,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 50; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0; + c.c = (c.c+c1.c)/50.0f; } driver->setPwm(0, 0, 0); gain_c *= _sign(c.c); diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 615c4733..748d3f44 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -48,17 +48,17 @@ class InlineCurrentSense: public CurrentSense{ int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio /** * Function finding zero offsets of the ADC */ void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 9618b572..057aa623 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -49,17 +49,17 @@ class LowsideCurrentSense: public CurrentSense{ int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio /** * Function finding zero offsets of the ADC */ void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 7ccd09fa..9ce089b8 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -75,9 +75,9 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver - Ua = _constrain(Ua, 0.0, voltage_limit); - Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Ua = _constrain(Ua, 0.0f, voltage_limit); + Ub = _constrain(Ub, 0.0f, voltage_limit); + Uc = _constrain(Uc, 0.0f, voltage_limit); // calculate duty cycle // limited in [0,1] dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a9432b1d..7cdd5045 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -89,8 +89,8 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); - duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); + duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); + duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index cfcb7aef..bf2681f6 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -66,14 +66,14 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); } \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index b0ac00e7..fc7cf98e 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,7 +4,7 @@ #if defined(_STM32_DEF_) // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_RANGE 4095// 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz From edce7d4d53880d4faf37533321e3eeb955918138 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:08:34 +0200 Subject: [PATCH 236/749] some refactoring and bug fixng --- .../alignment_and_cogging_test.ino | 2 + .../find_pole_pairs_number.ino | 2 + .../find_pole_pairs_number.ino | 4 +- .../encoder_example/encoder_example.ino | 4 ++ .../encoder_software_interrupts_example.ino | 4 ++ .../hall_sensor_example.ino | 3 ++ ...all_sensors_software_interrupt_example.ino | 3 ++ .../find_raw_min_max/find_raw_min_max.ino | 5 ++ .../magnetic_sensor_analog_example.ino} | 5 ++ .../esp32_i2c_dual_bus_example.ino | 7 +++ .../stm32_i2c_dual_bus_example.ino | 7 +++ .../magnetic_sensor_i2c_example.ino | 6 +++ .../find_raw_min_max/find_raw_min_max.ino | 5 ++ .../magnetic_sensor_pwm_example.ino} | 5 ++ ...magnetic_sensor_pwm_software_interrupt.ino | 5 ++ .../magnetic_sensor_spi_alt_example.ino | 5 ++ .../magnetic_sensor_spi_example.ino | 5 ++ keywords.txt | 3 ++ src/BLDCMotor.cpp | 53 +++++++++---------- src/StepperMotor.cpp | 41 +++++++++++--- src/common/base_classes/FOCMotor.cpp | 4 +- src/common/base_classes/Sensor.cpp | 6 +-- src/common/base_classes/Sensor.h | 30 +++++------ src/sensors/Encoder.cpp | 7 +-- src/sensors/Encoder.h | 2 +- src/sensors/HallSensor.cpp | 7 +-- src/sensors/HallSensor.h | 2 +- 27 files changed, 169 insertions(+), 63 deletions(-) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_analog_example => magnetic_sensor_analog}/find_raw_min_max/find_raw_min_max.ino (89%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino => magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino} (83%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_i2c}/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino (77%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_i2c}/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino (74%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_i2c}/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino (81%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example => magnetic_sensor_pwm}/find_raw_min_max/find_raw_min_max.ino (85%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino => magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino} (83%) rename examples/utils/sensor_test/magnetic_sensors/{magnetic_sensor_pwm_example => magnetic_sensor_pwm}/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino (85%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_spi}/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino (75%) rename examples/utils/sensor_test/magnetic_sensors/{ => magnetic_sensor_spi}/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino (76%) diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index ca254a5f..3fcd7b30 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -21,6 +21,7 @@ void testAlignmentAndCogging(int direction) { motor.move(0); _delay(200); + sensor.update(); float initialAngle = sensor.getAngle(); const int shaft_rotation = 720; // 720 deg test - useful to see repeating cog pattern @@ -40,6 +41,7 @@ void testAlignmentAndCogging(int direction) { _delay(5); // measure + sensor.update(); float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index e6187921..c5dfbf43 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -64,6 +64,7 @@ void setup() { motor.move(0); _delay(1000); // read the encoder angle + encoder.update(); float angle_begin = encoder.getAngle(); _delay(50); @@ -75,6 +76,7 @@ void setup() { } _delay(1000); // read the encoder value for 180 + encoder.update(); float angle_end = encoder.getAngle(); _delay(50); // turn off the motor diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index c2785c55..0b07c772 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -63,6 +63,7 @@ void setup() { motor.move(0); _delay(1000); // read the sensor angle + sensor.update(); float angle_begin = sensor.getAngle(); _delay(50); @@ -70,11 +71,12 @@ void setup() { float motor_angle = 0; while(motor_angle <= pp_search_angle){ motor_angle += 0.01f; - sensor.getAngle(); // keep track of the overflow + sensor.update(); // keep track of the overflow motor.move(motor_angle); } _delay(1000); // read the sensor value for 180 + sensor.update(); float angle_end = sensor.getAngle(); _delay(50); // turn off the motor diff --git a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino index 66aca6d8..f105f5b0 100644 --- a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino @@ -33,6 +33,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino index a979d9a0..ebf2dd41 100644 --- a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino @@ -46,6 +46,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index 6c19a2e9..ef2e504c 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -37,6 +37,9 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino index 523af03f..238eb2c4 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino @@ -49,6 +49,9 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino similarity index 89% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino index e45b0278..34e68a1b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino @@ -33,6 +33,11 @@ int max_count = 0; int min_count = 100000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.raw_count > max_count) max_count = sensor.raw_count; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino index 40d47ba8..2496151b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino @@ -25,6 +25,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino similarity index 77% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index c781847f..c930437b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -25,6 +25,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino similarity index 74% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino index 019e088c..08fb145d 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino @@ -24,6 +24,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino similarity index 81% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino index 30afc90c..4e060c0a 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino @@ -30,6 +30,12 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); + // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino index a88b7186..ac9bf044 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino @@ -27,6 +27,11 @@ int max_pulse= 0; int min_pulse = 10000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino index 15a1f557..6ae0a3e9 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino @@ -26,6 +26,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino index 6b19a9fa..10dc8a6b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -32,6 +32,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino similarity index 75% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino index 249837e6..713a19b1 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino similarity index 76% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino index 319c3741..048620ac 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/keywords.txt b/keywords.txt index 1d8156f5..b47c18fd 100644 --- a/keywords.txt +++ b/keywords.txt @@ -77,6 +77,9 @@ ISR KEYWORD2 getVelocity KEYWORD2 setPhaseVoltage KEYWORD2 getAngle KEYWORD2 +getMechanicalAngle KEYWORD2 +getSensorAngle KEYWORD2 +update KEYWORD2 needsSearch KEYWORD2 useMonitoring KEYWORD2 angleOpenloop KEYWORD2 diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8e9afe2c..7a25bdd8 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - sensor->updateSensor(); + sensor->update(); shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); @@ -165,7 +165,7 @@ int BLDCMotor::alignSensor() { _delay(2); } // take and angle in the middle - sensor->updateSensor(); + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { @@ -173,7 +173,7 @@ int BLDCMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } - sensor->updateSensor(); + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); @@ -204,8 +204,11 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - sensor->updateSensor(); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -234,7 +237,7 @@ int BLDCMotor::absoluteZeroSearch() { angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->updateSensor(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -254,23 +257,17 @@ int BLDCMotor::absoluteZeroSearch() { void BLDCMotor::loopFOC() { // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track // of full rotations otherwise. - if (sensor) sensor->updateSensor(); + if (sensor) sensor->update(); // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float - // For this reason it is NOT precise when the angles become large. - // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem - // when switching to a 2-component representation. - shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated - // if disabled do nothing if(!enabled) return; - // electrical angle - need shaftAngle to be called first - // TODO sensor precision: this will have precision issues because the shaft_angle does... + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); switch (torque_controller) { @@ -318,7 +315,16 @@ void BLDCMotor::move(float new_target) { // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; - // get angular velocity - sensor precision: this value is numerically precise. It is filtered by Velocity LPF, and downsamling (depending on sensor) + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing @@ -527,7 +533,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (modulation_centered) { - T0 = 1 - T1 - T2; //modulation_centered around driver->voltage_limit/2 + T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 } // calculate the duty cycles(times) @@ -595,8 +601,6 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // quick fix for strange cases (micros overflow + timestamp not defined) if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - // sensor precision: this calculation is numerically precise since we re-normalize - // the shaft-angle to the range 0-2PI // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes @@ -606,8 +610,6 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; - // sensor precision: this calculation is numerically precise, since shaft_angle is - // in the range 0-2PI // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -646,11 +648,8 @@ float BLDCMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - // TODO sensor precision: this calculation is not numerically precise. The angle is not constrained, - // and can grow large. It first gets multiplied by the number of pole-pairs (in _electrocalAngle()), - // and then gets normalized to 0-2PI (before it gets used in setPhaseVoltage()). At large angles, - // the precision can mean the range 0-2PI is not well represented at the end of that calculation. - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + // sensor precision: this calculation is OK due to the normalisation + setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index ca0716b7..2acd6f11 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -99,6 +99,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update + sensor->update(); shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); @@ -132,6 +133,7 @@ int StepperMotor::alignSensor() { _delay(2); } // take and angle in the middle + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { @@ -139,6 +141,7 @@ int StepperMotor::alignSensor() { setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); @@ -169,7 +172,11 @@ int StepperMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -197,7 +204,7 @@ int StepperMotor::absoluteZeroSearch() { angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -216,6 +223,11 @@ int StepperMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void StepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; // shaft angle @@ -224,6 +236,9 @@ void StepperMotor::loopFOC() { // if disabled do nothing if(!enabled) return; + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); // set the phase voltage - FOC heart function :) @@ -236,13 +251,25 @@ void StepperMotor::loopFOC() { // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { - // get angular velocity - shaft_velocity = shaftVelocity(); - // if disabled do nothing - if(!enabled) return; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + // set internal target variable if(_isset(new_target) ) target = new_target; // choose control loop @@ -370,7 +397,7 @@ float StepperMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index eae18e67..579f07ee 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -66,7 +66,9 @@ float FOCMotor::shaftVelocity() { } float FOCMotor::electricalAngle(){ - return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); + // if no sensor linked return previous value ( for open loop ) + if(!sensor) return electrical_angle; + return sensor_direction * _normalizeAngle(sensor->getMechanicalAngle() * pole_pairs - zero_electric_angle); } /** diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 1f6b649e..a5ad1b6d 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -4,7 +4,7 @@ // TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 -void Sensor::updateSensor() { +void Sensor::update() { float val = getSensorAngle(); angle_prev_ts = _micros(); float d_angle = val - angle_prev; @@ -14,7 +14,7 @@ void Sensor::updateSensor() { } - /** get current angular velocity (rad/s)*/ + /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; @@ -43,7 +43,7 @@ void Sensor::init() { } -float Sensor::getShaftAngle() { +float Sensor::getMechanicalAngle() { return angle_prev; } diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 47929cf3..3a946bc7 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -32,7 +32,7 @@ enum Pullup{ * method. getAngle() returns a float value, in radians, representing the current shaft angle in the * range 0 to 2*PI (one full turn). * - * To function correctly, the sensor class updateSensor() method has to be called sufficiently quickly. Normally, + * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally, * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly * enough, both for correct motor and sensor operation. * @@ -44,16 +44,16 @@ enum Pullup{ class Sensor{ public: /** - * Get shaft angle in the range 0 to 2PI. This value will be as precise as possible with - * the hardware. Base implementation uses the values returned by updateSensor() so that - * the same values are returned until updateSensor() is called again. + * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with + * the hardware. Base implementation uses the values returned by update() so that + * the same values are returned until update() is called again. */ - virtual float getShaftAngle(); + virtual float getMechanicalAngle(); /** * Get current position (in rad) including full rotations and shaft angle. - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. * Note that this value has limited precision as the number of rotations increases, * because the limited precision of float can't capture the large angle of the full * rotations and the small angle of the shaft angle at the same time. @@ -63,23 +63,23 @@ class Sensor{ /** * On architectures supporting it, this will return a double precision position value, * which should have improved precision for large position values. - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. */ virtual double getPreciseAngle(); /** * Get current angular velocity (rad/s) * Can be overridden in subclasses. Base implementation uses the values - * returned by updateSensor() so that it only makes sense to call this if updateSensor() + * returned by update() so that it only makes sense to call this if update() * has been called in the meantime. */ virtual float getVelocity(); /** * Get the number of full rotations - * Base implementation uses the values returned by updateSensor() so that the same - * values are returned until updateSensor() is called again. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. */ virtual int32_t getFullRotations(); @@ -92,10 +92,8 @@ class Sensor{ * rotations are not "missed" due to infrequent polling. * Override in subclasses if alternative behaviours are required for your * sensor hardware. - * - * Returns the same value as getAngle() as its result */ - virtual void updateSensor(); + virtual void update(); /** * returns 0 if it does need search for absolute zero @@ -110,7 +108,7 @@ class Sensor{ * * This method is pure virtual and must be implemented in subclasses. * Calling this method directly does not update the base-class internal fields. - * Use updateSensor() when calling from outside code. + * Use update() when calling from outside code. */ virtual float getSensorAngle()=0; /** diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 376c8092..9512e20a 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -102,10 +102,11 @@ void Encoder::handleIndex() { Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getShaftAngle(); + return getAngle(); } -float Encoder::getShaftAngle(){ - return _2PI * (pulse_counter % (int)cpr); +// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost +float Encoder::getMechanicalAngle(){ + return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); } float Encoder::getAngle(){ diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index c3cab450..b567fa4e 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,7 +61,7 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getShaftAngle() override; + float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; float getAngle() override; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index d6f24398..467ba9e7 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -95,16 +95,17 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { float HallSensor::getSensorAngle() { - return getShaftAngle(); + return getAngle(); } /* Shaft angle calculation + TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getShaftAngle() { - return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ; +float HallSensor::getMechanicalAngle() { + return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; } /* diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index ef991b77..9e80ed2b 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -55,7 +55,7 @@ class HallSensor: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getShaftAngle() override; + float getMechanicalAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; From 8b3046f083b6c7e7cd76a8c9f43588e586f3797e Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:09:26 +0200 Subject: [PATCH 237/749] quick readme update --- README.md | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index b9042658..1f83f36c 100644 --- a/README.md +++ b/README.md @@ -16,24 +16,8 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes be: SimpleFOClibrary v2.1.1 -> - bugfixes commander -> - bugfix `voltage_limit` when provided `phase_resistance` -> - bugfix `hall_sensor` examples -> - added examples fot the PowerShield -> - added initial support for `MagneticSensorPWM` -> - SAMD51 support -> - **initial support for Raspberry pi Pico** -> - added examples to find the raw max and min of the analog and pwm sensor -> - extension of the `Commander` interface -> - added pwm centering option `WC` -> - added pwm modulation cmd `WT` -> - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler -> - issue #62: motor.shaft_angle setting in the motor.initFOC() -> - issue #76: esp32 division by zero -> - issue #46: commander end of line character - by @maxlem -> - [community fix](https://community.simplefoc.com/t/as5600-dead-spot-around-0/208) AS5600 register value -> - renamed `Pullup::EXTERN` and `Pullup::INTERN` to `Pullup::USE_EXTERN` and `Pullup::USE_INTERN` +##### Release notes be: SimpleFOClibrary v2.1.2 +> - Sensor floating point error bugfux #83 ## Arduino *SimpleFOClibrary* v2.1 From f204ca5d30eebfdf57758aba4ec63c53ff870c7c Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:21:31 +0200 Subject: [PATCH 238/749] memory constraint arudino uno --- .../single_full_control_example/single_full_control_example.ino | 2 +- .../double_full_control_example/double_full_control_example.ino | 2 +- .../single_full_control_example/single_full_control_example.ino | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index 0b3075d3..dad3e18b 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -77,7 +77,7 @@ void setup() { command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); + Serial.println("Motor commands sketch | Initial motion control > torque/current : target 0Amps."); _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 3399d2fa..6024321b 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -115,7 +115,7 @@ void setup() { command.add('B', doMotor2, "motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Double motor sketch ready.")); + Serial.println("Double motor sketch ready."); _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 67d1d869..0762d940 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -76,7 +76,7 @@ void setup() { command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); _delay(1000); } From 7ed3868488fb51ee68bc0ba10b5ec1d872e7ce50 Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:40:09 +0200 Subject: [PATCH 239/749] Arduino UNO memeory shrinking --- README.md | 2 +- .../single_full_control_example.ino | 4 +++- .../double_full_control_example.ino | 12 ++++++++---- .../single_full_control_example.ino | 8 +++++--- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 1f83f36c..25983189 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes be: SimpleFOClibrary v2.1.2 +##### Release notes: SimpleFOClibrary v2.1.2 > - Sensor floating point error bugfux #83 ## Arduino *SimpleFOClibrary* v2.1 diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index dad3e18b..34c0883a 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -17,6 +17,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); // commander communication instance Commander command = Commander(Serial); void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { @@ -75,9 +76,10 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); + command.add('T', doTarget, "target"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/current : target 0Amps."); + Serial.println("Motor ready."); _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 6024321b..837602ab 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -32,8 +32,10 @@ InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); // commander communication instance Commander command = Commander(Serial); -void doMotor1(char* cmd){ command.motor(&motor1, cmd); } -void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +// void doMotor1(char* cmd){ command.motor(&motor1, cmd); } +// void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +void doTarget1(char* cmd){ command.scalar(&motor1.target, cmd); } +void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { @@ -111,8 +113,10 @@ void setup() { motor2.target = 2; // subscribe motor to the commander - command.add('A', doMotor1, "motor 1"); - command.add('B', doMotor2, "motor 2"); + // command.add('A', doMotor1, "motor 1"); + // command.add('B', doMotor2, "motor 2"); + command.add('A', doTarget1, "target motor 1"); + command.add('B', doTarget2, "target motor 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println("Double motor sketch ready."); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 0762d940..d6a00d60 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -17,7 +17,8 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -73,8 +74,9 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('M', doMotor, "motor"); - + command.add('T', doTarget, "target"); + // command.add('M', doMotor, "motor"); + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); From 361d043fbb656d7eb7ddda46f3898fd86993ac0d Mon Sep 17 00:00:00 2001 From: askuric Date: Tue, 20 Jul 2021 15:53:52 +0200 Subject: [PATCH 240/749] v3 memory optim for Arduino UNO --- .../single_full_control_example.ino | 4 ++-- .../double_full_control_example.ino | 6 +++--- .../single_full_control_example.ino | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index 34c0883a..dfad6e3a 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -16,7 +16,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { @@ -75,7 +75,7 @@ void setup() { motor.target = 0; // subscribe motor to the commander - command.add('M', doMotor, "motor"); + // command.add('M', doMotor, "motor"); command.add('T', doTarget, "target"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 837602ab..259b23ee 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -115,11 +115,11 @@ void setup() { // subscribe motor to the commander // command.add('A', doMotor1, "motor 1"); // command.add('B', doMotor2, "motor 2"); - command.add('A', doTarget1, "target motor 1"); - command.add('B', doTarget2, "target motor 2"); + command.add('A', doTarget1, "target 1"); + command.add('B', doTarget2, "target 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Double motor sketch ready."); + Serial.println("Motors ready."); _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index d6a00d60..2d6c3ca6 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -78,7 +78,7 @@ void setup() { // command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + Serial.println("Motor ready."); _delay(1000); } From 3d9fee867d41cdfe968654567bfff48d7bac9085 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Tue, 3 Aug 2021 08:14:14 +0200 Subject: [PATCH 241/749] Rename hall_sensors_software_interrupt_example.ino to hall_sensors_software_interrupts_example.ino --- ...t_example.ino => hall_sensors_software_interrupts_example.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/{hall_sensors_software_interrupt_example.ino => hall_sensors_software_interrupts_example.ino} (100%) diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino similarity index 100% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino From b7266ff431ca82974af07836fa0634dc871ab339 Mon Sep 17 00:00:00 2001 From: candyriver <37467728+candyriver@users.noreply.github.com> Date: Sat, 21 Aug 2021 14:35:24 +0800 Subject: [PATCH 242/749] Create atmega32u4_mcu.cpp add support for atmega32u4 --- .../hardware_specific/atmega32u4_mcu.cpp | 130 ++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 src/drivers/hardware_specific/atmega32u4_mcu.cpp diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp new file mode 100644 index 00000000..79cc9169 --- /dev/null +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -0,0 +1,130 @@ + +#include "../hardware_api.h" + +#if defined(__AVR_ATmega32U4__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html + if ( pin == 6 || pin == 13 ) { + TCCR4A=0x82; + TCCR4B=2; // TCCR4B=2 32khz; TCCR4B=1 64khz; TTCR4B=3 16khz + TCCR4C|=0x09; + TCCR4D=0; + } + if ( pin == 5 ){ + TCCR3B=1; + } + + if (pin == 9 || pin == 10 ){ + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + } + + if (pin == 3 || pin == 11 ) { + TCCR0B = ((TCCR0B & 0b11111000) | 0x01); // 62kHZ + //TCCR0B = ((TCCR0B & 0b11111000) | 0x02); // 7.8 khz + } +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + return -1; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif From d226061b325f5c86063eadf45d1a914db4ee059a Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 10:37:44 +0200 Subject: [PATCH 243/749] added support for leonardo #108 and some cases for stm32l0x achitecture --- src/common/time_utils.cpp | 4 +- .../hardware_specific/atmega2560_mcu.cpp | 2 +- .../hardware_specific/atmega32u4_mcu.cpp | 158 ++++++++++++++++++ src/drivers/hardware_specific/stm32_mcu.cpp | 27 ++- 4 files changed, 184 insertions(+), 7 deletions(-) create mode 100644 src/drivers/hardware_specific/atmega32u4_mcu.cpp diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp index 65ebad22..2acb47a3 100644 --- a/src/common/time_utils.cpp +++ b/src/common/time_utils.cpp @@ -3,7 +3,7 @@ // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips // use while instad of delay, // due to wrong measurement based on changed timer0 @@ -19,7 +19,7 @@ void _delay(unsigned long ms){ // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips //return the value based on the prescaler if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 3d50dc45..2d3c0305 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -7,7 +7,7 @@ void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 // https://forum.arduino.cc/index.php?topic=72092.0 - if (pin == 13 || pin == 4 ) { + if (pin == 13 || pin == 4 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp new file mode 100644 index 00000000..1442a434 --- /dev/null +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -0,0 +1,158 @@ + +#include "../hardware_api.h" + +#if defined(__AVR_ATmega32U4__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html + if (pin == 3 || pin == 11 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + else if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 5 ) + TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1 + else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13 + else if (pin == 13) TCCR4C |= 0x09; // Activate channel D - pin 6 + } + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + + if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){ + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + // configure complementary pwm on low side + if(pinH == 13 ){ + TCCR4A = 0x82; // activate channel A - pin 13 + TCCR4C |= 0x0d; // Activate complementary channel D - pin 6 + }else { + TCCR4C |= 0x09; // Activate channel D - pin 6 + TCCR4A = 0xc2; // activate complementary channel A - pin 13 + } + }else{ + return -1; + } + return 0; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index b0ac00e7..215ece87 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,7 +4,7 @@ #if defined(_STM32_DEF_) // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz @@ -70,10 +70,14 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCMode = TIM_OCMODE_PWM2; sConfigOC.Pulse = 100; sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#if defined(TIM_OCIDLESTATE_SET) + sConfig.OCIdleState = TIM_OCIDLESTATE_SET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); } @@ -121,6 +125,8 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha // configure hardware 6pwm interface only one timer with inverted channels HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { + +#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface PinName uhPinName = digitalPinToPinName(pinA_h); PinName ulPinName = digitalPinToPinName(pinA_l); PinName vhPinName = digitalPinToPinName(pinB_h); @@ -157,6 +163,9 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->refresh(); HT->resume(); return HT; +#else + return nullptr; // return nothing +#endif } @@ -176,12 +185,22 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + +#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface + + if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +#else // the rest of stm32 boards + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else return _ERROR_6PWM; // might be error avoid configuration +#endif } From 9f689db1224aaf131958adf24957b57dc6554ac5 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:35:32 +0200 Subject: [PATCH 244/749] added initial support for portenta, intial support for esp8266 + leonardo + esp32 separation from esp8266 --- .../hardware_specific/esp32_adc_driver.cpp | 2 +- .../hardware_specific/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32_mcu.cpp | 2 +- .../hardware_specific/atmega32u4_mcu.cpp | 27 +- src/drivers/hardware_specific/esp32_mcu.cpp | 2 +- src/drivers/hardware_specific/esp8266_mcu.cpp | 77 +++ src/drivers/hardware_specific/generic_mcu.cpp | 2 +- .../hardware_specific/portenta_h7_mcu.cpp | 539 ++++++++++++++++++ 8 files changed, 641 insertions(+), 12 deletions(-) create mode 100644 src/drivers/hardware_specific/esp8266_mcu.cpp create mode 100644 src/drivers/hardware_specific/portenta_h7_mcu.cpp diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp index 01e1ca60..76ee54e9 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h index a01eef19..df8166f0 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index d4ac0587..7b9ba448 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" #include "../../drivers/hardware_api.h" -#ifdef ESP_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 1442a434..fc782dfa 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -17,7 +17,7 @@ void _pinHighFrequency(const int pin){ TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1 else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer // PLL Configuration - PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode @@ -93,11 +93,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); @@ -114,7 +111,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){ // PLL Configuration - PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode @@ -132,6 +129,22 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return 0; } + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + // function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index c87e3b70..f6663d87 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(ESP_H) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp new file mode 100644 index 00000000..ccc31aa2 --- /dev/null +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -0,0 +1,77 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFreq(freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); +} +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 5cc8bc4b..f89c0562 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" // if the mcu doen't have defiend analogWrite -#if defined(ESP_H) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp new file mode 100644 index 00000000..74a66348 --- /dev/null +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -0,0 +1,539 @@ + +#include "../hardware_api.h" + +#if 1 //defined(TARGET_PORTENTA_H7) + +#include "pwmout_api.h" +#include "pinDefinitions.h" +#include "platform/mbed_critical.h" +#include "platform/mbed_power_mgmt.h" +#include "platform/mbed_assert.h" +#include "PeripheralPins.h" +#include "pwmout_device.h" + +// default pwm parameters +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +// // 6pwm parameters +// #define _HARDWARE_6PWM 1 +// #define _SOFTWARE_6PWM 0 +// #define _ERROR_6PWM -1 + +typedef struct{ + int id ; + pwmout_t pins[6]; +} portenta_h7_mcu_enty_s; + +portenta_h7_mcu_enty_s motor_slot[10]; +int slot_index = 0; + + +/* Convert STM32 Cube HAL channel to LL channel */ +uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj) +{ +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1N; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2N; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4N; +#endif + default : /* Optional */ + return 0; + } + } else +#endif + { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3; + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4; + default : /* Optional */ + return 0; + } + } +} + + + +// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ +// return _pwm_init(obj, digitalPinToPinName(pin), frequency); +// } + +int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ + int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM); + int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM); + + const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function}; + + pwmout_init_direct(obj, &static_pinmap); + + TIM_HandleTypeDef TimHandle; + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + RCC_ClkInitTypeDef RCC_ClkInitStruct; + uint32_t PclkFreq = 0; + uint32_t APBxCLKDivider = RCC_HCLK_DIV1; + uint8_t i = 0; + + + __HAL_TIM_DISABLE(&TimHandle); + + // Get clock configuration + // Note: PclkFreq contains here the Latency (not used after) + HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq); + + /* Parse the pwm / apb mapping table to find the right entry */ + while (pwm_apb_map_table[i].pwm != obj->pwm) i++; + // sanity check + if (pwm_apb_map_table[i].pwm == 0) return -1; + + + if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) { + PclkFreq = HAL_RCC_GetPCLK1Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider; + } else { +#if !defined(PWMOUT_APB2_NOT_SUPPORTED) + PclkFreq = HAL_RCC_GetPCLK2Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider; +#endif + } + + long period_us = 500000.0/((float)frequency); + /* By default use, 1us as SW pre-scaler */ + obj->prescaler = 1; + // TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick + } + TimHandle.Init.Period = (period_us - 1); + + /* In case period or pre-scalers are out of range, loop-in to get valid values */ + while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) { + obj->prescaler = obj->prescaler * 2; + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1; + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1; + } + TimHandle.Init.Period = (period_us - 1) / obj->prescaler; + /* Period decreases and prescaler increases over loops, so check for + * possible out of range cases */ + if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) { + break; + } + } + + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned + + if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { + return -1; + } + + TIM_OC_InitTypeDef sConfig; + // Configure channels + sConfig.OCMode = TIM_OCMODE_PWM1; + sConfig.Pulse = obj->pulse / obj->prescaler; + sConfig.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfig.OCFastMode = TIM_OCFAST_DISABLE; +#if defined(TIM_OCIDLESTATE_RESET) + sConfig.OCIdleState = TIM_OCIDLESTATE_RESET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif + + int channel = 0; + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return -1; + } + + if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) { + // If channel is not enabled, proceed to channel configuration + if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) { + return -1; + } + } + + // Save for future use + obj->period = period_us; +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + HAL_TIMEx_PWMN_Start(&TimHandle, channel); + } else +#endif + { + HAL_TIM_PWM_Start(&TimHandle, channel); + } + + return 0; +} + +// setting pwm to hardware pin - instead analogWrite() +void _pwm_write(pwmout_t *obj, float value){ + TIM_HandleTypeDef TimHandle; + int channel = 0; + + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + + if (value < (float)0.0) { + value = 0.0; + } else if (value > (float)1.0) { + value = 1.0; + } + + obj->pulse = (uint32_t)((float)obj->period * value + 0.5); + + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return; + } + + // If channel already enabled, only update compare value to avoid glitch + __HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler); +} + +// init low side pin +// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +// { +// PinName pin = digitalPinToPinName(ulPin); +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); +// sConfigOC.OCMode = TIM_OCMODE_PWM2; +// sConfigOC.Pulse = 100; +// sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; +// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; +// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; +// sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; +// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); +// } +// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); +// HT->setOverflow(PWM_freq, HERTZ_FORMAT); +// HT->pause(); +// HT->refresh(); +// return HT; +// } + + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){ + TIM_HandleTypeDef TimHandle1, TimHandle2; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_DISABLE(&TimHandle4); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle4); +} + +// // configure hardware 6pwm interface only one timer with inverted channels +// HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +// { +// PinName uhPinName = digitalPinToPinName(pinA_h); +// PinName ulPinName = digitalPinToPinName(pinA_l); +// PinName vhPinName = digitalPinToPinName(pinB_h); +// PinName vlPinName = digitalPinToPinName(pinB_l); +// PinName whPinName = digitalPinToPinName(pinC_h); +// PinName wlPinName = digitalPinToPinName(pinC_l); + +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); +// } +// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + +// // dead time is set in nanoseconds +// uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; +// uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); +// LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! +// LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + +// HT->pause(); +// HT->refresh(); +// HT->resume(); +// return HT; +// } + + +// // returns 0 if each pair of pwm channels has same channel +// // returns 1 all the channels belong to the same timer - hardware inverted channels +// // returns -1 if neither - avoid configuring - error!!! +// int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +// PinName nameAH = digitalPinToPinName(pinA_h); +// PinName nameAL = digitalPinToPinName(pinA_l); +// PinName nameBH = digitalPinToPinName(pinB_h); +// PinName nameBL = digitalPinToPinName(pinB_l); +// PinName nameCH = digitalPinToPinName(pinC_h); +// PinName nameCL = digitalPinToPinName(pinC_l); +// int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); +// int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); +// int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); +// int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); +// int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); +// int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); +// if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) +// return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer +// else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) +// return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer +// else +// return _ERROR_6PWM; // might be error avoid configuration +// } + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + core_util_critical_section_exit(); + slot_index++; +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + core_util_critical_section_exit(); + slot_index++; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3])); + core_util_critical_section_exit(); + slot_index++; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +for(int i=0; i Date: Wed, 25 Aug 2021 13:36:50 +0200 Subject: [PATCH 245/749] separation esp32 + esp8266 --- src/sensors/MagneticSensorSPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 6ba31d15..b7a9dd27 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -128,7 +128,7 @@ word MagneticSensorSPI::read(word angle_register){ spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); -#if defined( ESP_H ) // if ESP32 board +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) // if ESP32 board delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 From d518987373d11d0b9fec14d09558b397470f73a5 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:37:46 +0200 Subject: [PATCH 246/749] pwm frequency not set initially --- src/drivers/BLDCDriver6PWM.cpp | 1 + src/drivers/StepperDriver2PWM.cpp | 1 + src/drivers/StepperDriver4PWM.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 6039f69e..eaeb2cfa 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,6 +15,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; // dead zone initial - 2% dead_zone = 0.02f; diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a9432b1d..92092556 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -36,6 +36,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index cfcb7aef..f7918cc2 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -14,6 +14,7 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } From 9251630bfccf4ae8f171e8a5f63045790cb9f09b Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:46:42 +0200 Subject: [PATCH 247/749] remove default params from checkbus, encoder added empty update, bug in FOCMotor::electricalAngle --- src/BLDCMotor.cpp | 1 + src/common/base_classes/FOCMotor.cpp | 2 +- src/sensors/Encoder.cpp | 5 +++++ src/sensors/Encoder.h | 3 ++- src/sensors/MagneticSensorI2C.h | 2 +- 5 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 3ae98893..b0c84ba4 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -209,6 +209,7 @@ int BLDCMotor::alignSensor() { // get the current zero electric angle zero_electric_angle = 0; zero_electric_angle = electricalAngle(); + //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 579f07ee..41c0dfd2 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -68,7 +68,7 @@ float FOCMotor::shaftVelocity() { float FOCMotor::electricalAngle(){ // if no sensor linked return previous value ( for open loop ) if(!sensor) return electrical_angle; - return sensor_direction * _normalizeAngle(sensor->getMechanicalAngle() * pole_pairs - zero_electric_angle); + return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle ); } /** diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 9512e20a..7c164878 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -98,6 +98,11 @@ void Encoder::handleIndex() { } } + +void Encoder::update() { + // do nothing for Encoder +} + /* Shaft angle calculation */ diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index b567fa4e..3e775338 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -27,7 +27,7 @@ class Encoder: public Sensor{ Encoder(int encA, int encB , float ppr, int index = 0); /** encoder initialise pins */ - void init(); + void init() override; /** * function enabling hardware interrupts for the encoder channels with provided callback functions * if callback is not provided then the interrupt is not enabled @@ -67,6 +67,7 @@ class Encoder: public Sensor{ float getAngle() override; double getPreciseAngle() override; int32_t getFullRotations() override; + virtual void update() override; /** * returns 0 if it does need search for absolute zero diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 29bebee4..fa7ec96b 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -49,7 +49,7 @@ class MagneticSensorI2C: public Sensor{ float getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ - int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + int checkBus(byte sda_pin , byte scl_pin ); private: float cpr; //!< Maximum range of the magnetic sensor From 3a4ea20e5ab9c159a4205d50490d271c3a6304f1 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:49:56 +0200 Subject: [PATCH 248/749] update readme + library properties --- README.md | 7 ++++++- library.properties | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 25983189..fe7ab263 100644 --- a/README.md +++ b/README.md @@ -16,8 +16,13 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes: SimpleFOClibrary v2.1.2 +##### Release notes: SimpleFOClibrary v2.2 > - Sensor floating point error bugfux #83 +> - Support for portenta h7 board +> - Support for arduino leonardo #108 +> - Support for esp8266 +> - Low side current sensing support for esp32 +> - Restructured the generic code and simplified adding new mcus ## Arduino *SimpleFOClibrary* v2.1 diff --git a/library.properties b/library.properties index 0d89185d..89540636 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.2 +version=2.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 63d56321d33e4dd33600590fd3a1ed512041d3a2 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:51:22 +0200 Subject: [PATCH 249/749] forgotten pwm_frequency default --- src/drivers/BLDCDriver3PWM.cpp | 1 + src/drivers/StepperDriver2PWM.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 88d5ab95..bc239875 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -14,6 +14,7 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 8881709f..a0e61ca0 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -16,6 +16,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } From a71072945f2fac46f4d0180c0d58e08e179e7c12 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 13:56:56 +0200 Subject: [PATCH 250/749] forgotten define portenta --- src/drivers/hardware_specific/portenta_h7_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index 74a66348..b314ee40 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if 1 //defined(TARGET_PORTENTA_H7) +#if defined(TARGET_PORTENTA_H7) #include "pwmout_api.h" #include "pinDefinitions.h" From 8a86629308e074958cc5678a3e1d213340caf4c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Wed, 25 Aug 2021 14:07:20 +0200 Subject: [PATCH 251/749] stm32 bugfix --- src/drivers/hardware_specific/stm32_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 215ece87..5efddf41 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -72,11 +72,11 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; #if defined(TIM_OCIDLESTATE_SET) - sConfig.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; #endif #if defined(TIM_OCNIDLESTATE_RESET) - sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; #endif uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); From a55da2da8dd1cb1ab770fe515288dc85b90f2f7a Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 28 Apr 2021 16:46:37 +0200 Subject: [PATCH 252/749] Implement current sense for STM32 based B-G431B-ESC1 for this board, use InlineCurrentSense with: shunt_resistor = 0.003 gain = -64.0/7.0 --- .../hardware_specific/generic_mcu.cpp | 1 - .../hardware_specific/stm32_mcu.cpp | 2 +- .../hardware_specific/stm32g4_hal.cpp | 333 ++++++++++++++++++ .../hardware_specific/stm32g4_hal.h | 14 + .../hardware_specific/stm32g4_mcu.cpp | 122 +++++++ src/drivers/hardware_specific/stm32_mcu.cpp | 9 + 6 files changed, 479 insertions(+), 2 deletions(-) create mode 100644 src/current_sense/hardware_specific/stm32g4_hal.cpp create mode 100644 src/current_sense/hardware_specific/stm32g4_hal.h create mode 100644 src/current_sense/hardware_specific/stm32g4_mcu.cpp diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index ecc0b79b..50f383ad 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -26,7 +26,6 @@ __attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,c if( _isset(pinC) ) pinMode(pinC, INPUT); } - // sync driver and the adc __attribute__((weak)) void _driverSyncLowSide(){ } __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp index 19833e11..797ac6a1 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) and !defined(STM32G4xx) #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f diff --git a/src/current_sense/hardware_specific/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32g4_hal.cpp new file mode 100644 index 00000000..f0204566 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_hal.cpp @@ -0,0 +1,333 @@ +#include "../hardware_api.h" +#if defined(STM32G4xx) + +#include "stm32g4xx_hal.h" +#include "stm32g4xx_ll_pwr.h" +#include "stm32g4xx_hal_adc.h" +#include "stm32g4_hal.h" +// From STM32 cube IDE +/** + ****************************************************************************** + * @attention + * + *

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

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

    NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

    +
      +
    • Sensor floating point error bugfix (initial solution) #83, #37
      • +
    • Sensor class restructuring - slight API change - docs
      • +
    • Restructured the generic code and simplified adding new mcus: IMPORTANT: an additional compiler flag needed for PlatformIO see issue and PlatformIO docs
      • +
    • Removed initial jump #110, #111
      • +
    • Double to float transformation of the code - performance increase by @sDessens (#100), @KaSroka (#100)
      • +
    • Docs webiste translated to Chinese! 🎉: Awesome work 😃 by @MINQING1101, @Deng-ge-open-source and @mingggggggg
      • +
    • New MCU support +
        +
      • Support for arduino leonardo #108 - docs
        • +
      • Initial support for portenta h7 board in collaboration with Arduino - docs
        • +
      • Initial support for esp8266 - docs
        • +
      +
      • +
    • Low side current sensing initial support - docs +
        +
      • Initial support for stm32 B_G431B_ESC1 by @sDessens: PR #73
        • +
      • Initial support for samd21 by @maxlem: PR #79
        • +
      • Initial support for esp32 by @byDagor
        • +
      +
      • +
    +
    ## Arduino *SimpleFOClibrary* v2.1 From ed900161371baeba3cf9e3186f649fe50229f2f2 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 3 Oct 2021 18:02:18 +0200 Subject: [PATCH 275/749] fix typo in css --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index bf5072c8..c0e019d6 100644 --- a/README.md +++ b/README.md @@ -26,11 +26,11 @@ Therefore this is an attempt to:

    @@ -80,7 +80,7 @@ Therefore this is an attempt to:

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

    NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

    +

    FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

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

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

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

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

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

    FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

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

      FUTURE RELEASE 📢: SimpleFOClibrary v2.2.1

      +

      NEW RELEASE 📢: SimpleFOClibrary v2.2.1 see release

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

    NEW RELEASE 📢: SimpleFOClibrary v2.2.1 see release

    +

    NEXT RELEASE 📢: SimpleFOClibrary v2.2.2 see release