diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 3f98490..5ec83f0 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -13,64 +13,116 @@ jobs: name: Test compile runs-on: ubuntu-latest strategy: - matrix: - arduino-boards-fqbn: - - arduino:avr:nano # arudino nano - - 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:esp32s2 # esp32s2 - - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - arduino:mbed_rp2040:pico # rpi pico - include: - - arduino-boards-fqbn: arduino:avr:nano - sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage - required-libraries: Simple FOC - - arduino-boards-fqbn: arduino:sam:arduino_due_x - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - - arduino-boards-fqbn: arduino:samd:nano_33_iot - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing - - arduino-boards-fqbn: arduino:mbed_rp2040:pico - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 - platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 - # platform-url: https://dl.espressif.com/dl/package_esp32_index.json - # required-libraries: Simple FOC - # sketch-names: '**.ino' - - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega linearhall - - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - required-libraries: Simple FOC - sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage simplefocnano_atmega - - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - required-libraries: Simple FOC - sketches-exclude: smoothing simplefocnano_torque_voltage simplefocnano_atmega - # Do not cancel all jobs / architectures if one job fails fail-fast: false + matrix: + board: + - fqbn: arduino:avr:nano # arudino nano + platforms: | + - name: arduino:avr + sketch-paths: | + - examples/drivers/simplefocnano/simplefocnano_atmega + - examples/encoders/linearhall + report-name-suffix: arduino_avr_nano + - fqbn: arduino:sam:arduino_due_x # arduino due + platforms: | + - name: arduino:sam + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: arduino_sam_due + - fqbn: arduino:samd:nano_33_iot # samd21 + platforms: | + - name: arduino:samd + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: arduino_samd_nano_33_iot + - fqbn: adafruit:samd:adafruit_metro_m4 # samd51 + platforms: | + - name: adafruit:samd + source-url: "https://adafruit.github.io/arduino-board-index/package_adafruit_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: adafruit_samd_metro_m4 + - fqbn: esp32:esp32:esp32 # esp32 + platforms: | + - name: esp32:esp32 + source-url: "https://espressif.github.io/arduino-esp32/package_esp32_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/mt6816 + report-name-suffix: esp32_esp32 + - fqbn: esp32:esp32:esp32s2 # esp32s2 + platforms: | + - name: esp32:esp32 + source-url: "https://espressif.github.io/arduino-esp32/package_esp32_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: esp32_esp32s2 + - fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + platforms: | + - name: STMicroelectronics:stm32 + source-url: "https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + report-name-suffix: stm32_genf1_bluepill_f103c8 + - fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + platforms: | + - name: STMicroelectronics:stm32 + source-url: "https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: stm32_nucleo_f411re + - fqbn: rp2040:rp2040:rpipico # rpi pico + platforms: | + - name: rp2040:rp2040 + source-url: "https://github.com/earlephilhower/arduino-pico/releases/download/global/package_rp2040_index.json" + sketch-paths: | + - examples/drivers/drv8316 + - examples/encoders/calibrated_sensor/calibration_save + - examples/encoders/linearhall + - examples/encoders/mt6816 + report-name-suffix: arduino_mbed_rp2040_pico steps: - name: Checkout uses: actions/checkout@master - name: Compile all examples - uses: ArminJo/arduino-test-compile@master + uses: arduino/compile-sketches@v1 + with: + fqbn: ${{ matrix.board.fqbn }} + platforms: ${{ matrix.board.platforms }} + libraries: | + - name: "Arduino-FOC" + source-url: "https://github.com/simplefoc/Arduino-FOC.git" + version: dev + - name: "Arduino-FOC-Drivers" + source-path: . + sketch-paths: ${{ matrix.board.sketch-paths || 'examples' }} + enable-deltas-report: true + sketches-report-path: sketches-reports + - name: Upload sketches reports + uses: actions/upload-artifact@v4 with: - arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - required-libraries: ${{ matrix.required-libraries }} - platform-url: ${{ matrix.platform-url }} - sketch-names: ${{ matrix.sketch-names }} - sketches-exclude: ${{ matrix.sketches-exclude }} + name: sketches-report-${{ matrix.board.report-name-suffix }} + path: sketches-reports + report: + runs-on: ubuntu-latest + steps: + - name: Report size deltas + uses: arduino/report-size-deltas@v1 diff --git a/.github/workflows/report-size-deltas.yml b/.github/workflows/report-size-deltas.yml new file mode 100644 index 0000000..ce7644e --- /dev/null +++ b/.github/workflows/report-size-deltas.yml @@ -0,0 +1,11 @@ +name: Report Size Deltas +on: + schedule: + - cron: "*/5 * * * *" +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: arduino/report-size-deltas@v1 + with: + sketches-reports-source: ^sketches-report-.+ diff --git a/README.md b/README.md index a87a1e1..d3ad0a6 100644 --- a/README.md +++ b/README.md @@ -10,13 +10,19 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.8 - Released July 2024, for Simple FOC 2.3.4 or later +v1.0.9 - Released July 2025, for Simple FOC 2.3.5 or later -What's changed since 1.0.7? -- MA735 driver thanks to [@techyrobot](https://github.com/techy-robot) -- ESP32HWEncoder driver thanks to [@mcells](https://github.com/mcells) -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.8+) +What's changed since 1.0.8? +- FluxObserverSensor for sensorless FOC thanks to [@Candas1](https://github.com/Candas1) +- AS5600 fast mode support +- Improvements to SmoothingSensor and LinearHall thanks to [@dekutree64](https://github.com/dekutree64) +- Improvements to CalibratedSensor and HybridStepper thanks to [@askuric](https://github.com/askuric) +- AS5600 driver bugfix thanks to [@zbas](https://github.com/zbas) +- Calibrated sensor improvements thanks to [@Schnilz](https://github.com/Schnilz) +- ESP32HWEncoder bugfix thanks to [@AndBindStyle](https://github.com/AndBondStyle) +- STM32HWEncoder fix thanks to [@AntonEvmenenko](https://github.com/AntonEvmenenko) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.9) ## What is included @@ -116,6 +122,11 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi ## Release History +What's changed since 1.0.7? +- MA735 driver thanks to [@techyrobot](https://github.com/techy-robot) +- ESP32HWEncoder driver thanks to [@mcells](https://github.com/mcells) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.8) + What's changed since 1.0.6? - Improvements to LinearHall driver, thanks to dekutree - Fix for ESP32 compiler warning, thanks to Yannik Stradmann @@ -126,14 +137,14 @@ What's changed since 1.0.6? - Refactored settings storage code - Refactored I2CCommander - New utility class for simple trapezoidal motion profiles -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7+) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7) What's changed since 1.0.5? - Added STSPIN32G4 driver - Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs - Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs - Improvements in the MT6835 sensor driver -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6) What's changed since 1.0.4? - Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) @@ -143,7 +154,7 @@ What's changed since 1.0.4? - New Settings driver: SAMDNVMSettingsStorage - SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" - Updated I2CCommander to use the new registers abstraction -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5) What's changed since 1.0.3? - New Comms/Input: STM32SpeedDirCommander diff --git a/examples/encoders/calibrated/calibrated.ino b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino similarity index 84% rename from examples/encoders/calibrated/calibrated.ino rename to examples/encoders/calibrated_sensor/calibrated/calibrated.ino index 6cc6110..6e4e303 100644 --- a/examples/encoders/calibrated/calibrated.ino +++ b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino @@ -1,10 +1,5 @@ /** - * Torque control example using voltage control loop. - * - * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers - * you a way to control motor torque by setting the voltage to the motor instead hte current. - * - * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + * The example demonstrates the usage of the calibrated sensor object. */ #include #include @@ -16,6 +11,8 @@ MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6); BLDCMotor motor = BLDCMotor(11); BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); // instantiate the calibrated sensor object +// argument 1 - sensor object +// argument 2 - number of samples in the LUT (default 200) CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); // voltage set point variable @@ -58,6 +55,7 @@ void setup() { // set voltage to run calibration sensor_calibrated.voltage_calibration = 6; // Running calibration + // it will ouptut the LUT and the zero electrical angle to the serial monitor !!!! sensor_calibrated.calibrate(motor); //Serial.println("Calibrating Sensor Done."); diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino new file mode 100644 index 0000000..da49b73 --- /dev/null +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -0,0 +1,97 @@ +/** + * The example demonstrates the calibration of the magnetic sensor with the calibration procedure and saving the calibration data. + * So that the calibration procedure does not have to be run every time the motor is powered up. + * + * 1. Run this Sketch as is and wait for the calibration data to be generated and printed over Serial. + * 2. Then copy the output from Serial to calibrationLut, zero_electric_angle and sensor_direction + * 3. Change values_provided to true and run the Sketch again to see the motor skipping the calibration. + */ + +#include +#include +#include "encoders/calibrated/CalibratedSensor.h" + +// fill this array with the calibration values outputed by the calibration procedure +float calibrationLut[50] = {0}; +float zero_electric_angle = 0; +Direction sensor_direction = Direction::UNKNOWN; + +const bool values_provided = false; + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14); +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +StepperDriver4PWM driver = StepperDriver4PWM(10, 9, 5, 6,8); +// instantiate the calibrated sensor object +// instantiate the calibrated sensor object +// argument 1 - sensor object +// argument 2 - number of samples in the LUT (default 200) +// argument 3 - pointer to the LUT array (defualt nullptr) +CalibratedSensor sensor_calibrated = CalibratedSensor( + sensor, sizeof(calibrationLut) / sizeof(calibrationLut[0])); + +// voltage set point variable +float target_voltage = 2; + +// instantiate the commander +Commander command = Commander(Serial); + +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + sensor.init(); + // Link motor to sensor + motor.linkSensor(&sensor); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + motor.init(); + + if(values_provided) { + motor.zero_electric_angle = zero_electric_angle; + motor.sensor_direction = sensor_direction; + } else { + // Running calibration + sensor_calibrated.calibrate(motor); + } + + // Linking sensor to motor object + motor.linkSensor(&sensor_calibrated); + + // calibrated init FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + +void loop() { + + motor.loopFOC(); + motor.move(target_voltage); + command.run(); + motor.monitor(); + +} \ No newline at end of file diff --git a/examples/encoders/smoothing/smoothing.ino b/examples/encoders/smoothing/smoothing.ino index 1ff658d..f84a205 100644 --- a/examples/encoders/smoothing/smoothing.ino +++ b/examples/encoders/smoothing/smoothing.ino @@ -1,6 +1,9 @@ /** * - * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor + * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor. + * The only changes are the declaration of the SmoothingSensor, passing it to motor.linkSensor + * instead of the HallSensor instance, and the added Commander code to switch between the two. + * * Steps: * 1) Configure the motor and sensor * 2) Run the code @@ -63,8 +66,6 @@ void setup() { sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenerIndex); - // set SmoothingSensor phase correction for hall sensors - smooth.phase_correction = -_PI_6; // link the SmoothingSensor to the motor motor.linkSensor(&smooth); diff --git a/library.properties b/library.properties index 30a9c8d..f3234cb 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.8 +version=1.0.9 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. diff --git a/src/comms/i2c/I2CCommanderMaster.cpp b/src/comms/i2c/I2CCommanderMaster.cpp index 666e009..fa237f6 100644 --- a/src/comms/i2c/I2CCommanderMaster.cpp +++ b/src/comms/i2c/I2CCommanderMaster.cpp @@ -50,8 +50,11 @@ int I2CCommanderMaster::readRegister(int motor, SimpleFOCRegister registerNum, v int I2CCommanderMaster::readLastUsedRegister(int motor, void* data, uint8_t size){ int numRead = motors[motor].wire->requestFrom(motors[motor].address, size); - if (numRead==size) - motors[motor].wire->readBytes((uint8_t*)data, size); + if (numRead==size) { + for (int i=0; i < size; i++) { + ((uint8_t*)data)[i] = motors[motor].wire->read(); + } + } else { return 0; } diff --git a/src/comms/stm32speeddir/README.md b/src/comms/stm32speeddir/README.md index 1e36a5a..40ad39a 100644 --- a/src/comms/stm32speeddir/README.md +++ b/src/comms/stm32speeddir/README.md @@ -21,6 +21,10 @@ The direction input is optional - if not provided, you can control the direction The velocity values returned are in the range `min_speed` to `max_speed`, while the input PWM duty cycle should lie within the range `min_pwm` to `max_pwm`. Actual input values smaller than `min_pwm` will be treated as `min_pwm`, values larger than `max_pwm` will be treated as `max_pwm`. The behaviour for 100% or 0% duty cycles is undefined, and they should be avoided. +> **IMPORTANT**
+> If the PWM frequency of the speed input is not known, provide its value in Hz to the constructor. If not provided, it will default to 1kHz (very common value). The frequency is used to make sure that the PWM period stays within one timer counter period. If this is not the case the timer counter can overflow and the input will not work correctly. + + ## Usage Use it like this: @@ -31,8 +35,9 @@ Use it like this: // some example pins - the speed pin has to be on channel 1 or 2 of a timer #define PIN_SPEED PC6 #define PIN_DIRECTION PB8 +#define PWM_FREQUENCY 1000 // 1kHz (defualt) -STM32SpeedDirInput speed_dir = STM32SpeedDirInput(PIN_SPEED, PIN_DIRECTION); +STM32SpeedDirInput speed_dir = STM32SpeedDirInput(PIN_SPEED, PIN_DIRECTION, PWM_FREQUENCY); float target = 0.0f; diff --git a/src/comms/stm32speeddir/STM32SpeedDirInput.cpp b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp index 4bc8bee..86f9060 100644 --- a/src/comms/stm32speeddir/STM32SpeedDirInput.cpp +++ b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp @@ -3,7 +3,7 @@ #if defined(_STM32_DEF_) -STM32SpeedDirInput::STM32SpeedDirInput(int pin_speed, int pin_dir) : STM32PWMInput(pin_speed) { +STM32SpeedDirInput::STM32SpeedDirInput(int pin_speed, int pin_dir, uint32_t pwm_freq) : STM32PWMInput(pin_speed, _pwm_freq) { _pin_speed = pin_speed; _pin_dir = pin_dir; }; diff --git a/src/comms/stm32speeddir/STM32SpeedDirInput.h b/src/comms/stm32speeddir/STM32SpeedDirInput.h index 3a3647b..b897877 100644 --- a/src/comms/stm32speeddir/STM32SpeedDirInput.h +++ b/src/comms/stm32speeddir/STM32SpeedDirInput.h @@ -10,7 +10,21 @@ class STM32SpeedDirInput : public STM32PWMInput { public: - STM32SpeedDirInput(int pin_speed, int pin_dir = NOT_SET); + + /** + * STM32SpeedDirInput constructor + * + * @param pin_speed - the pin number to read the speed PWM signal from + * @param pin_dir - the pin number to read the direction signal from (default is NOT_SET) + * @param pwm_freq - the frequency of the PWM signal (default is 1kHz) + * + * This class is used to read speed and direction signals from a pin using the STM32 HAL library. + * IMPORTANT! + * This class can only be used with the pins that are associated with some timer, + * and only if they are associated to the channels 1 or 2. The timer can not be + * used for other purposes, like motor control. + */ + STM32SpeedDirInput(int pin_speed, int pin_dir = NOT_SET, uint32_t pwm_freq = 1000); ~STM32SpeedDirInput(); int init(); diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp index e438b0b..5394ed5 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp @@ -1,7 +1,7 @@ #include "./SimpleFOCNanoDriver.h" -#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) SimpleFOCNanoDriver::SimpleFOCNanoDriver() : BLDCDriver3PWM(PIN_INU, PIN_INV, PIN_INW, PIN_ENU, PIN_ENV, PIN_ENW) { // nothing to do here diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h index f0917c7..7c63632 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h @@ -4,7 +4,7 @@ #include -#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) /* * Default pins for the SimpleFOC Nano board diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp new file mode 100644 index 0000000..cf872d1 --- /dev/null +++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp @@ -0,0 +1,104 @@ +#include "MXLEMMINGObserverSensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + + +MXLEMMINGObserverSensor::MXLEMMINGObserverSensor(const FOCMotor& m) : _motor(m) +{ + // Derive Flux linkage from KV_rating and pole_pairs + if (_isset(_motor.pole_pairs) && _isset(_motor.KV_rating)){ + flux_linkage = 60 / ( _SQRT3 * _PI * _motor.KV_rating * _motor.pole_pairs * 2); + } +} + + +void MXLEMMINGObserverSensor::update() { + // Current sense is required for the observer + if (!_motor.current_sense) return; + + // Exit if one of the parameter needed for the flux observer is 0 + if ((_motor.phase_inductance == 0) || + (_motor.phase_resistance == 0) || + (flux_linkage == 0)) return; + + // Update sensor, with optional downsampling of update rate + if (sensor_cnt++ < sensor_downsample) return; + + sensor_cnt = 0; + + // read current phase currents + PhaseCurrent_s current = _motor.current_sense->getPhaseCurrents(); + + // calculate clarke transform + ABCurrent_s ABcurrent = _motor.current_sense->getABCurrents(current); + + // get current timestamp + long now_us = _micros(); + // calculate the sample time from last call + float dt = (now_us - angle_prev_ts) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if(dt <= 0 || dt > 0.5f) dt = 1e-3f; + + // This work deviates slightly from the BSD 3 clause licence. + // The work here is entirely original to the MESC FOC project, and not based + // on any appnotes, or borrowed from another project. This work is free to + // use, as granted in BSD 3 clause, with the exception that this note must + // be included in where this code is implemented/modified to use your + // variable names, structures containing variables or other minor + // rearrangements in place of the original names I have chosen, and credit + // to David Molony as the original author must be noted. + + // MXLEMMING Flux Observer + float resistive_term_a = _motor.phase_resistance * ABcurrent.alpha; + float resistive_term_b = _motor.phase_resistance * ABcurrent.beta; + float inductive_term_a = _motor.phase_inductance * (ABcurrent.alpha - i_alpha_prev); + float inductive_term_b = _motor.phase_inductance * (ABcurrent.beta - i_beta_prev); + + flux_alpha = _constrain( flux_alpha + (_motor.Ualpha - resistive_term_a) * dt - inductive_term_a ,-flux_linkage, flux_linkage); + flux_beta = _constrain( flux_beta + (_motor.Ubeta - resistive_term_b) * dt - inductive_term_b ,-flux_linkage, flux_linkage); + + // Calculate electrical angle + electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha)); + + // Electrical angle difference + float d_electrical_angle = electrical_angle - electrical_angle_prev; + if(abs(d_electrical_angle) > _2PI * 0.8 ){ //change the factor based on sample rate can also just use _PI for simplicity + if (d_electrical_angle > 0){ + d_electrical_angle -= _2PI; + }else{ + d_electrical_angle += _2PI; + } + } + angle_track += d_electrical_angle; + + // Mechanical angle and full_rotations + float full_rotation = _2PI * _motor.pole_pairs; + if(abs(angle_track) > full_rotation){ + if (angle_track>0){ + full_rotations += 1; + angle_track -= full_rotation; + }else{ + full_rotations -= 1; + angle_track += full_rotation; + } + } + angle_prev = angle_track /_motor.pole_pairs; + + // Store Previous values + i_alpha_prev = ABcurrent.alpha; + i_beta_prev = ABcurrent.beta; + angle_prev_ts = now_us; + electrical_angle_prev = electrical_angle; + +} + +void MXLEMMINGObserverSensor::init(){ + this->Sensor::init(); // call base class +} + +/* + Shaft angle calculation +*/ +float MXLEMMINGObserverSensor::getSensorAngle(){ + return 0; +} \ No newline at end of file diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h new file mode 100644 index 0000000..e5ba28c --- /dev/null +++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h @@ -0,0 +1,46 @@ +#ifndef MXLEMMING_OBSERVER_SENSOR_H +#define MXLEMMING_OBSERVER_SENSOR_H + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" + +/** + +*/ + +class MXLEMMINGObserverSensor : public Sensor +{ + public: + /** + MXLEMMINGObserverSensor class constructor + @param m Motor that the MXLEMMINGObserverSensor will be linked to + */ + MXLEMMINGObserverSensor(const FOCMotor& m); + void update() override; + + void init() override; + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getSensorAngle() override; + + + // For sensors with slow communication, use these to poll less often + unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update + unsigned int sensor_cnt = 0; // counting variable for downsampling + float flux_alpha = 0; // Flux Alpha + float flux_beta = 0; // Flux Beta + float flux_linkage = 0; // Flux linkage, calculated based on KV and pole number + float i_alpha_prev = 0; // Previous Alpha current + float i_beta_prev = 0; // Previous Beta current + float electrical_angle = 0; // Electrical angle + float electrical_angle_prev = 0; // Previous electrical angle + float angle_track = 0; // Total Electrical angle + + protected: + const FOCMotor& _motor; + +}; + +#endif \ No newline at end of file diff --git a/src/encoders/MXLEMMING_observer/README.md b/src/encoders/MXLEMMING_observer/README.md new file mode 100644 index 0000000..29a06b5 --- /dev/null +++ b/src/encoders/MXLEMMING_observer/README.md @@ -0,0 +1,39 @@ +# MXLEMMING Observer Sensor + +The MXLEMMING Obserser has been ported from the MESC Firmware, it's also the default Flux observer in Vesc firmware. +The [MESC book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer) explains the math behind this flux observer. + +It's a simple solution for sensorless motor control only using phase currents and motor parameters, if tracking the position at low speed and when not driving the motor is not relevant. + +### Motor Parameters: +The MXLEMMING Observer needs the following motor parameters to be set: +- phase resistance +- KV rating +- phase inductance +- pole pairs + +It will not track the position if any of those parameters are missing. + +The KV rating and pole pairs parameters are used to derive the motor flux linkage which is key for the flux observer to run well. +``` +#include + +BLDCMotor motor = BLDCMotor(15, 0.1664, 17.0, 0.00036858); // Hoverboard Motor +MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor); +``` +flux_linkage parameter can be adjusted from the code. + +### Current Sense +The current sense is required as this flux observer only relies on phase currents. + +### Sensor Alignment +The flux observer sensor doesn't need sensor alignment. +``` +motor.sensor_direction= Direction::CW; +motor.zero_electric_angle = 0; +``` + +### To do: +- The Clarke transform is running both in the loopFOC and in the sensor update now, it can be remove from the sensor when the Alpha and Beta currents will be persisted as a BLDCMotor member +- The flux observer is calculating the electrical angle directly, but SimpleFOC needs to derive the electrical angle from the sensor angle for the FOC calculation + diff --git a/src/encoders/as5600/AS5600.cpp b/src/encoders/as5600/AS5600.cpp index c95a6e6..109ad19 100644 --- a/src/encoders/as5600/AS5600.cpp +++ b/src/encoders/as5600/AS5600.cpp @@ -124,13 +124,13 @@ void AS5600::setZPos(uint16_t value) { void AS5600::setI2CAddr(uint8_t value) { uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1); val = (value<<1) | (val&0x01); - writeRegister(AS5600_REG_I2CADDR, val); + writeRegister(AS5600_REG_I2CADDR, val, 1); }; void AS5600::setI2CUpdt(uint8_t value) { uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1); val = (value<<1) | (val&0x01); - writeRegister(AS5600_REG_I2CUPDT, val); + writeRegister(AS5600_REG_I2CUPDT, val, 1); }; diff --git a/src/encoders/as5600/MagneticSensorAS5600.cpp b/src/encoders/as5600/MagneticSensorAS5600.cpp index 4cea5cf..556c762 100644 --- a/src/encoders/as5600/MagneticSensorAS5600.cpp +++ b/src/encoders/as5600/MagneticSensorAS5600.cpp @@ -13,6 +13,6 @@ void MagneticSensorAS5600::init(TwoWire* wire) { float MagneticSensorAS5600::getSensorAngle() { - uint16_t raw = readRawAngle(); + uint16_t raw = angle(); return raw / AS5600_CPR * _2PI; }; \ No newline at end of file diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index e3d579c..089b17a 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -1,257 +1,307 @@ #include "CalibratedSensor.h" - // CalibratedSensor() // sensor - instance of original sensor object -CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) -{ -}; - - -CalibratedSensor::~CalibratedSensor() -{ - delete calibrationLut; +// n_lut - number of samples in the LUT +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float *lut) + : _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) { + }; + +CalibratedSensor::~CalibratedSensor() { + // delete calibrationLut; + if(allocated) { + delete []calibrationLut; + } }; // call update of calibrated sensor -void CalibratedSensor::update(){ - _wrapped.update(); - this->Sensor::update(); +void CalibratedSensor::update() +{ + _wrapped.update(); + this->Sensor::update(); }; // Retrieve the calibrated sensor angle -void CalibratedSensor::init() { - // assume wrapped sensor has already been initialized - this->Sensor::init(); // call superclass init +void CalibratedSensor::init() +{ + // assume wrapped sensor has already been initialized + this->Sensor::init(); // call superclass init } // Retrieve the calibrated sensor angle -float CalibratedSensor::getSensorAngle(){ +float CalibratedSensor::getSensorAngle() +{ + if(!calibrationLut) { + return _wrapped.getMechanicalAngle(); + } // raw encoder position e.g. 0-2PI - float rawAngle = _wrapped.getMechanicalAngle(); + float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI); + raw_angle += raw_angle < 0 ? _2PI:0; + + // Calculate the resolution of the LUT in radians + float lut_resolution = _2PI / n_lut; + // Calculate LUT index + int lut_index = raw_angle / lut_resolution; + + // Get calibration values from the LUT + float y0 = calibrationLut[lut_index]; + float y1 = calibrationLut[(lut_index + 1) % n_lut]; + + // Linearly interpolate between the y0 and y1 values + // Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1) + // If distance = 0, interpolated offset = y0 + // If distance = 1, interpolated offset = y1 + float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution; + float offset = (1 - distance) * y0 + distance * y1; + + // Calculate the calibrated angle + return raw_angle - offset; +} - // index of the bucket that rawAngle is part of. - // e.g. rawAngle = 0 --> bucketIndex = 0. - // e.g. rawAngle = 2PI --> bucketIndex = 128. - int bucketIndex = floor(rawAngle/(_2PI/n_lut)); - float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex); +// Perform filtering to linearize position sensor eccentricity +// FIR n-sample average, where n = number of samples in the window +// This filter has zero gain at electrical frequency and all integer multiples +// So cogging effects should be completely filtered out +void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ + float window_buffer[window]; + memset(window_buffer, 0, window*sizeof(float)); + float window_sum = 0; + int buffer_index = 0; + // fill the inital window buffer + for (int i = 0; i < window; i++) { + int ind = n_ticks - window/2 -1 + i; + window_buffer[i] = error[ind % n_ticks]; + window_sum += window_buffer[i]; + } + // calculate the moving average + error_mean = 0; + for (int i = 0; i < n_ticks; i++) + { + // Update buffer + window_sum -= window_buffer[buffer_index]; + window_buffer[buffer_index] = error[( i + window/2 ) %n_ticks]; + window_sum += window_buffer[buffer_index]; + // update the buffer index + buffer_index = (buffer_index + 1) % window; + + // Update filtered error + error[i] = window_sum / (float)window; + // update the mean value + error_mean += error[i] / n_ticks; + } - // Extract the lower and upper LUT value in counts - float y0 = calibrationLut[bucketIndex]; - float y1 = calibrationLut[(bucketIndex+1)%n_lut]; +} - // Linear Interpolation Between LUT values y0 and y1 using the remainder - // If remainder = 0, interpolated offset = y0 - // If remainder = 2PI/n_lut, interpolated offset = y1 - float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; +void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) +{ + // if the LUT is already defined, skip the calibration - // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians - float calibratedAngle = rawAngle+interpolatedOffset; + if(calibrationLut == NULL) { + allocated = true; + calibrationLut = new float[n_lut]; + } + motor.monitor_port->println("Starting Sensor Calibration."); - // return calibrated angle in radians - return calibratedAngle; -} + // Calibration variables + + // Init inital angles + float theta_actual = 0.0; + float avg_elec_angle = 0.0; + // set the inital electric angle to 0 + float elec_angle = 0.0; + + // Calibration parameters + // The motor will take a n_pos samples per electrical cycle + // which amounts to n_ticks (n_pos * motor.pole_pairs) samples per mechanical rotation + // Additionally, the motor will take n2_ticks steps to reach any of the n_ticks posiitons + // incrementing the electrical angle by deltaElectricalAngle each time + int n_pos = 5; + int _NPP = motor.pole_pairs; // number of pole pairs which is user input + const int n_ticks = n_pos * _NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2_ticks = 5; // increments between saved samples (for smoothing motion) + float deltaElectricalAngle = _2PI * _NPP / (n_ticks * n2_ticks); // Electrical Angle increments for calibration steps + float error[n_ticks]; // pointer to error array (average of forward & backward) + memset(error, 0, n_ticks*sizeof(float)); + // The fileter window size is set to n_pos - one electrical cycle + // important for cogging filtering !!! + const int window = n_pos; // window size for moving average filter of raw error + -void CalibratedSensor::calibrate(BLDCMotor& motor){ - - Serial.println("Starting Sensor Calibration."); - - int _NPP = motor.pole_pairs; // number of pole pairs which is user input - const int n_ticks = 128*_NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) - const int n2_ticks = 40; // increments between saved samples (for smoothing motion) - float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks); // Electrical Angle increments for calibration steps - float* error_f = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_f = new float[n_ticks](); // pointer to raw forward position - float* error_b = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_b = new float[n_ticks](); // pointer to raw backword position - float* error = new float[n_ticks](); // pointer to error array (average of forward & backward) - float* error_filt = new float[n_ticks](); // pointer to filtered error array (low pass filter) - const int window = 128; // window size for moving average filter of raw error - motor.zero_electric_angle = 0; // Set position sensor offset - - // find natural direction (this is copy of the init code) - // move one electrical revolution forward - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0f; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - // take and angle in the middle - _wrapped.update(); - float mid_angle = _wrapped.getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0f ; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - _wrapped.update(); - float end_angle = _wrapped.getAngle(); - motor.setPhaseVoltage(0, 0, 0); - _delay(200); - // determine the direction the sensor moved - int directionSensor; - if (mid_angle < end_angle) { - Serial.println("MOT: sensor_direction==CCW"); - directionSensor = -1; - motor.sensor_direction = Direction::CCW; - - } else{ - Serial.println("MOT: sensor_direction==CW"); - directionSensor = 1; - motor.sensor_direction = Direction::CW; - - } - - //Set voltage angle to zero, wait for rotor position to settle - // keep the motor in position while getting the initial positions - motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); - _delay(1000); - _wrapped.update(); - float theta_init = _wrapped.getAngle(); - float theta_absolute_init = _wrapped.getMechanicalAngle(); - - /* - Start Calibration - Loop over electrical angles from 0 to NPP*2PI, once forward, once backward - store actual position and error as compared to electrical angle - */ - - /* + // find the first guess of the motor.zero_electric_angle + // and the sensor direction + // updates motor.zero_electric_angle + // updates motor.sensor_direction + // temporarily unlink the sensor and current sense + CurrentSense *current_sense = motor.current_sense; + motor.current_sense = nullptr; + motor.linkSensor(&this->_wrapped); + if(!motor.initFOC()){ + motor.monitor_port->println("Failed to align the sensor."); + return; + } + // link back the sensor and current sense + motor.linkSensor(this); + motor.linkCurrentSense(current_sense); + + // Set voltage angle to zero, wait for rotor position to settle + // keep the motor in position while getting the initial positions + motor.setPhaseVoltage(1, 0, elec_angle); + _delay(1000); + _wrapped.update(); + float theta_init = _wrapped.getAngle(); + float theta_absolute_init = _wrapped.getMechanicalAngle(); + + /* + Start Calibration + Loop over electrical angles from 0 to NPP*2PI, once forward, once backward + store actual position and error as compared to electrical angle + */ + + /* forwards rotation */ - Serial.println("Rotating forwards"); - int k = 0; - for(int i = 0; iprint("Rotating: "); + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CCW" : "CW" ); + float zero_angle_prev = 0.0; + for (int i = 0; i < n_ticks; i++) + { + for (int j = 0; j < n2_ticks; j++) // move to the next location { - theta_actual = -theta_actual; - error_f[i] = theta_actual - elec_angle/_NPP; - + _wrapped.update(); + elec_angle += deltaElectricalAngle; + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); } - else - { - error_f[i] = elec_angle/_NPP - theta_actual; + + // delay to settle in position before taking a position sample + _delay(settle_time_ms); + _wrapped.update(); + // calculate the error + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); + error[i] = 0.5 * (theta_actual - elec_angle / _NPP); + + // calculate the current electrical zero angle + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + zero_angle = _normalizeAngle(zero_angle); + // remove the 2PI jumps + if(zero_angle - zero_angle_prev > _PI){ + zero_angle = zero_angle - _2PI; + }else if(zero_angle - zero_angle_prev < -_PI){ + zero_angle = zero_angle + _2PI; } - // if overflow happened track it as full rotation -// raw_f[i] = theta_actual; - - // storing the normalized angle every time the electrical angle 3PI/2 to calculate average zero electrical angle - if(i==(k*128+96)) - { - _delay(50); - avg_elec_angle += _normalizeAngle(directionSensor*_wrapped.getMechanicalAngle()*_NPP); - k += 1; - } + zero_angle_prev = zero_angle; + avg_elec_angle += zero_angle/n_ticks; + + // motor.monitor_port->print(">zero:"); + // motor.monitor_port->println(zero_angle); + // motor.monitor_port->print(">zero_average:"); + // motor.monitor_port->println(avg_elec_angle/2.0); } - _delay(2000); - /* + /* backwards rotation */ - Serial.println("Rotating backwards"); - for(int i = 0; iprint("Rotating: "); + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CW" : "CCW" ); + for (int i = n_ticks - 1; i >= 0; i--) + { + for (int j = 0; j < n2_ticks; j++) // move to the next location + { _wrapped.update(); - theta_actual = _wrapped.getAngle()-theta_init; - if (directionSensor == -1) - { - theta_actual = -theta_actual; - error_b[i] = theta_actual - elec_angle/_NPP; - } - else - { - error_b[i] = elec_angle/_NPP - theta_actual; - } -// raw_b[i] = theta_actual; + elec_angle -= deltaElectricalAngle; + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + } + + // delay to settle in position before taking a position sample + _delay(settle_time_ms); + _wrapped.update(); + // calculate the error + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); + error[i] += 0.5 * (theta_actual - elec_angle / _NPP); + // calculate the current electrical zero angle + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + zero_angle = _normalizeAngle(zero_angle); + // remove the 2PI jumps + if(zero_angle - zero_angle_prev > _PI){ + zero_angle = zero_angle - _2PI; + }else if(zero_angle - zero_angle_prev < -_PI){ + zero_angle = zero_angle + _2PI; + } + zero_angle_prev = zero_angle; + avg_elec_angle += zero_angle/n_ticks; + + // motor.monitor_port->print(">zero:"); + // motor.monitor_port->println(zero_angle); + // motor.monitor_port->print(">zero_average:"); + // motor.monitor_port->println(avg_elec_angle/2.0); } // get post calibration mechanical angle. _wrapped.update(); - theta_absolute_post = _wrapped.getMechanicalAngle(); + float theta_absolute_post = _wrapped.getMechanicalAngle(); // done with the measurement motor.setPhaseVoltage(0, 0, 0); // raw offset from initial position in absolute radians between 0-2PI - float raw_offset = (theta_absolute_init+theta_absolute_post)/2; + float raw_offset = (theta_absolute_init + theta_absolute_post) / 2; + + // calculating the average zero electrical angle from the forward calibration. + motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); + motor.monitor_port->print("Average Zero Electrical Angle: "); + motor.monitor_port->println(motor.zero_electric_angle); + _delay(1000); - // calculating the average zero electrica angle from the forward calibration. - motor.zero_electric_angle = avg_elec_angle/_NPP; - Serial.print( "Average Zero Electrical Angle: "); - Serial.println( motor.zero_electric_angle); - // Perform filtering to linearize position sensor eccentricity // FIR n-sample average, where n = number of samples in one electrical cycle // This filter has zero gain at electrical frequency and all integer multiples // So cogging effects should be completely filtered out - float mean = 0; - for (int i = 0; i n_ticks-1) { - ind -= n_ticks;} - error_filt[i] += error[ind]/(float)window; - } - mean += error_filt[i]/n_ticks; - } + float error_mean = 0.0; + this->filter_error(error, error_mean, n_ticks, window); + _delay(1000); // calculate offset index - int index_offset = floor(raw_offset/(_2PI/n_lut)); + int index_offset = floor((float)n_lut * raw_offset / _2PI); + float dn = n_ticks / (float)n_lut; + motor.monitor_port->println("Constructing LUT."); + _delay(1000); // Build Look Up Table - for (int i = 0; i (n_lut-1)){ - ind -= n_lut; - } - if(ind < 0 ){ - ind += n_lut; - } - calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean); - //Serial.print(ind); - //Serial.print('\t'); - //Serial.println(calibrationLut[ind],5); + for (int i = 0; i < n_lut; i++) + { + int ind = index_offset + i*motor.sensor_direction; + if (ind > (n_lut - 1)) ind -= n_lut; + if (ind < 0) ind += n_lut; + calibrationLut[ind] = (float)(error[(int)(i * dn)] - error_mean); + // negate the error if the sensor is in the opposite direction + calibrationLut[ind] = (int)motor.sensor_direction * calibrationLut[ind]; + } + motor.monitor_port->println(""); + _delay(1000); + + // Display the LUT + motor.monitor_port->print("float calibrationLut["); + motor.monitor_port->print(n_lut); + motor.monitor_port->println("] = {"); + _delay(100); + for (int i=0;i < n_lut; i++){ + motor.monitor_port->print(calibrationLut[i],6); + if(i < n_lut - 1) motor.monitor_port->print(", "); _delay(1); } - - // de-allocate memory - delete error_filt; - delete error; - // delete raw_b; - delete error_b; - // delete raw_f; - delete error_f; + motor.monitor_port->println("};"); + _delay(1000); - Serial.println("Sensor Calibration Done."); + // Display the zero electrical angle + motor.monitor_port->print("float zero_electric_angle = "); + motor.monitor_port->print(motor.zero_electric_angle,6); + motor.monitor_port->println(";"); -} + // Display the sensor direction + motor.monitor_port->print("Direction sensor_direction = "); + motor.monitor_port->println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;"); + _delay(1000); + motor.monitor_port->println("Sensor Calibration Done."); +} diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index 414c53a..29a0a8d 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -4,13 +4,20 @@ #include "common/base_classes/Sensor.h" #include "BLDCMotor.h" #include "common/base_classes/FOCMotor.h" +#include "common/foc_utils.h" class CalibratedSensor: public Sensor{ public: - // constructor of class with pointer to base class sensor and driver - CalibratedSensor(Sensor& wrapped); + /** + * @brief Constructor of class with pointer to base class sensor and driver + * @param wrapped the wrapped sensor which needs calibration + * @param n_lut the number of entries in the lut + * @param lut the look up table (if null, the lut will be allocated on the heap) + */ + CalibratedSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr); + ~CalibratedSensor(); /* @@ -21,11 +28,10 @@ class CalibratedSensor: public Sensor{ /** * Calibrate method computes the LUT for the correction */ - virtual void calibrate(BLDCMotor& motor); + virtual void calibrate(FOCMotor& motor, int settle_time_ms = 30); // voltage to run the calibration: user input - float voltage_calibration = 1; - + float voltage_calibration = 1; protected: /** @@ -42,20 +48,17 @@ class CalibratedSensor: public Sensor{ * delegate instance of Sensor class */ Sensor& _wrapped; + + void alignSensor(FOCMotor &motor); + void filter_error(float* error, float &error_mean, int n_ticks, int window); - // lut size, currently constan. Perhaps to be made variable by user? - const int n_lut { 128 } ; - // create pointer for lut memory - float* calibrationLut = new float[n_lut](); - - // Init inital angles - float theta_actual { 0.0 }; - float elecAngle { 0.0 }; - float elec_angle { 0.0 }; - float theta_absolute_post { 0.0 }; - float theta_absolute_init { 0.0 }; - float theta_init { 0.0 }; - float avg_elec_angle { 0.0 }; + // lut size - settable by the user + int n_lut { 200 } ; + // pointer for lut memory + // depending on the size of the lut + // will be allocated in the calibrate function if not given. + bool allocated; + float* calibrationLut; }; -#endif \ No newline at end of file +#endif diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index 5768d92..9a1c823 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -81,11 +81,69 @@ void setup() { Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-drivers/blob/master/examples/encoders/calibrated/sensor_calibration.ino) in our examples directory. +## EDIT March 2025 -## Roadmap +The code has been rewritten to reduce its memory footprint and allow more flexible Lookup table (LUT) sizing. +Additionally, the calibrated sensor class now supports providing the saved LUT as a paramer for calibration. This allows you to save the LUT and load it on startup to avoid recalibration on each startup. -Possible future improvements we've thought about: +Once you do the calibration once, it will output something like this: -- Improve memory usage and performance -- Make calibration able to be saved/restored +``` +... + +Starting Sensor Calibration. +MOT: Align sensor. +MOT: sensor_direction==CCW +MOT: PP check: OK! +MOT: Zero elec. angle: 3.17 +MOT: No current sense. +MOT: Ready.Rotating: CCW +Rotating: CW +Average Zero Electrical Angle: 4.01 +Constructing LUT. + +float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; +float zero_electric_angle = 4.007072; +Direction sensor_direction = Direction::CCW; +Sensor Calibration Done +... +``` + +The LUT and sensor's zero angle and direction are outputed by the calibration process to the Serial terminal. So you can copy and paste them into your code. + +Your code will look something like this: + +```c++ + +// number of LUT entries +const N_LUT = 50; +// Lookup table that has been ouptut from the calibration process +float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; +float zero_electric_angle = 4.007072; +Direction sensor_direction = Direction::CCW; + +// provide the sensor class and the number of points in the LUT +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT); + +... + +void setup() { + ... + // as LUT is provided to this function + sensor_calibrated.calibrate(motor, calibrationLut, zero_eletrical_angle, sensor_direction); + ... + + motor.linkSensor(&sensor_calibrated); + + ... + motor.initFOC(); + .... +} + + +``` + +## Future work +- Reduce the LUT size by using a more efficient LUT type - maybe pass to uint16_t +- Use a more eficient LUT interpolation method - maybe a polynomial interpolation \ No newline at end of file diff --git a/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp index 29fcb9c..8126043 100644 --- a/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp +++ b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp @@ -108,7 +108,9 @@ void ESP32HWEncoder::init() overflowISR_args[pcnt_config.unit].set = 1; // Register and enable the interrupt - pcnt_isr_register(overflowCounter, (void*)&overflowISR_args, 0, (pcnt_isr_handle_t*) NULL); + uint8_t count = 0; + for (uint8_t i = 0; i < PCNT_UNIT_MAX; i++) count += used_units[i]; + if (count == 1) pcnt_isr_register(overflowCounter, (void*)&overflowISR_args, 0, (pcnt_isr_handle_t*) NULL); pcnt_intr_enable(pcnt_config.unit); // Just check the last command for errors diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp index 96cfae3..ad2efae 100644 --- a/src/encoders/linearhall/LinearHall.cpp +++ b/src/encoders/linearhall/LinearHall.cpp @@ -13,6 +13,8 @@ LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ pinA = _hallA; pinB = _hallB; pp = _pp; + electrical_rev = 0; + prev_reading = 0; } float LinearHall::getSensorAngle() { @@ -84,7 +86,7 @@ void LinearHall::init(FOCMotor *motor) { // move one mechanical revolution forward for (int i = 0; i <= 2000; i++) { - float angle = _3PI_2 + _2PI * i * pp / 2000.0f; + float angle = _3PI_2 + _2PI * i * motor->pole_pairs / pp / 2000.0f; motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); ReadLinearHalls(pinA, pinB, &lastA, &lastB); diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp index ef0d322..85d7f00 100644 --- a/src/encoders/smoothing/SmoothingSensor.cpp +++ b/src/encoders/smoothing/SmoothingSensor.cpp @@ -1,12 +1,16 @@ #include "SmoothingSensor.h" #include "common/foc_utils.h" #include "common/time_utils.h" +#include "sensors/HallSensor.h" SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) { } +SmoothingSensor::SmoothingSensor(HallSensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) { + phase_correction = -_PI_6; +} void SmoothingSensor::update() { // Update sensor, with optional downsampling of update rate @@ -27,8 +31,8 @@ void SmoothingSensor::update() { // Apply phase correction if needed if (phase_correction != 0) { - if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs; - else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs; + if (_motor.shaft_velocity < -0.001) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs; + else if (_motor.shaft_velocity > 0.001) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs; } // Handle wraparound of the projected angle diff --git a/src/encoders/smoothing/SmoothingSensor.h b/src/encoders/smoothing/SmoothingSensor.h index c10c2a0..af38343 100644 --- a/src/encoders/smoothing/SmoothingSensor.h +++ b/src/encoders/smoothing/SmoothingSensor.h @@ -22,6 +22,7 @@ class SmoothingSensor : public Sensor @param m Motor that the SmoothingSensor will be linked to */ SmoothingSensor(Sensor& s, const FOCMotor& m); + SmoothingSensor(class HallSensor& s, const FOCMotor& m); // Automatically sets phase_correction void update() override; float getVelocity() override; diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp index 456cf75..1cea602 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp @@ -2,14 +2,17 @@ #if defined(_STM32_DEF_) +#include "drivers/hardware_specific/stm32/stm32_mcu.h" + /* HardwareEncoder(int cpr) */ -STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int8_t pinA, int8_t pinB, int8_t pinI) { +STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int pinA, int pinB, int pinI) { cpr = _ppr * 4; // 4x for quadrature _pinA = digitalPinToPinName(pinA); _pinB = digitalPinToPinName(pinB); _pinI = digitalPinToPinName(pinI); + index_found = false; } /* @@ -20,10 +23,10 @@ float STM32HWEncoder::getSensorAngle() { } // getter for index pin -int STM32HWEncoder::needsSearch() { return false; } +int STM32HWEncoder::needsSearch() { return false && !index_found; } // private function used to determine if encoder has index -int STM32HWEncoder::hasIndex() { return 0; } +int STM32HWEncoder::hasIndex() { return (_pinI!=NC); } // encoder initialisation of the hardware pins void STM32HWEncoder::init() { @@ -74,11 +77,21 @@ void STM32HWEncoder::init() { return; } + // TODO on STM32G4 MCUs we can use the TIMx_ETR pin for the index, and configure how it is handled automatically by the hardware + // on non-G4 MCUs we need to use an external interrupt to handle the index signal + // attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() { + // encoder_handle.Instance->CNT = 0; // reset counter + // index_found = true; + // // detach interrupt + // detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI))); + // }, index_polarity); + if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) { initialized = false; return; } + stm32_reserveTimer(encoder_handle.Instance); initialized = true; } diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h index 98b14e5..eb8ec5c 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.h +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h @@ -16,7 +16,7 @@ class STM32HWEncoder : public Sensor { Encoder class constructor @param ppr impulses per rotation (cpr=ppr*4) */ - explicit STM32HWEncoder(unsigned int ppr, int8_t pinA, int8_t pinB, int8_t pinI=-1); + explicit STM32HWEncoder(unsigned int ppr, int pinA, int pinB, int pinI=-1); void init() override; int needsSearch() override; @@ -25,6 +25,8 @@ class STM32HWEncoder : public Sensor { bool initialized = false; uint32_t cpr; //!< encoder cpr number PinName _pinA, _pinB, _pinI; + bool index_found; + uint32_t index_polarity = RISING; protected: float getSensorAngle() override; diff --git a/src/encoders/stm32lpencoder/PinMap_LPTIM.c b/src/encoders/stm32lpencoder/PinMap_LPTIM.cpp similarity index 95% rename from src/encoders/stm32lpencoder/PinMap_LPTIM.c rename to src/encoders/stm32lpencoder/PinMap_LPTIM.cpp index 53dcd1b..f21af53 100644 --- a/src/encoders/stm32lpencoder/PinMap_LPTIM.c +++ b/src/encoders/stm32lpencoder/PinMap_LPTIM.cpp @@ -1,6 +1,7 @@ #include "Arduino.h" -#ifdef HAL_LPTIM_MODULE_ENABLED + +#if defined(_STM32_DEF_) && defined(HAL_LPTIM_MODULE_ENABLED) // TODO which models does this apply to? the entire series, or the line? diff --git a/src/encoders/stm32pwmsensor/README.md b/src/encoders/stm32pwmsensor/README.md index fd98be9..e173533 100644 --- a/src/encoders/stm32pwmsensor/README.md +++ b/src/encoders/stm32pwmsensor/README.md @@ -21,10 +21,18 @@ sensor.getDutyCycleTicks(); By rotating the motor through several full turns while printing the ticks to the screen you will be able to determine the correct values empirically. + +> **IMPORTANT**
+> If the PWM frequency of the speed input is not known, provide its value in Hz to the constructor. If not provided, it will default to 1kHz (very common value). The frequency is used to make sure that the PWM period stays within one timer counter period. If this is not the case the timer counter can overflow and the input will not work correctly. + + ## Usage ``` -STM32MagneticSensorPWM sensor = STM32MagneticSensorPWM(PB7, 412, 6917); // sample values, yours will be different + +#define PWM_FREQ_HZ 1000 // 1kHz (default) + +STM32MagneticSensorPWM sensor = STM32MagneticSensorPWM(PB7, 412, 6917, PWM_FREQ_HZ); // sample values, yours will be different void setup() { ... diff --git a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp index 3c3e1d6..20dd8bf 100644 --- a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp +++ b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp @@ -6,7 +6,7 @@ #include "common/foc_utils.h" -STM32MagneticSensorPWM::STM32MagneticSensorPWM(int pin, uint32_t _min_ticks, uint32_t _max_ticks) : STM32PWMInput(pin), max_ticks(_max_ticks), min_ticks(_min_ticks) { +STM32MagneticSensorPWM::STM32MagneticSensorPWM(int pin, uint32_t _min_ticks, uint32_t _max_ticks, uint32_t _pwm_freq) : STM32PWMInput(pin), max_ticks(_max_ticks), min_ticks(_min_ticks) { }; diff --git a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h index 3e5d335..36db6f3 100644 --- a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h +++ b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h @@ -13,7 +13,7 @@ class STM32MagneticSensorPWM : public Sensor, public STM32PWMInput { public: - STM32MagneticSensorPWM(int pin, uint32_t _min_ticks = 0, uint32_t _max_ticks = 0x0FFF); + STM32MagneticSensorPWM(int pin, uint32_t _min_ticks = 0, uint32_t _max_ticks = 0x0FFF, uint32_t _pwm_freq = 1000); ~STM32MagneticSensorPWM(); void init() override; diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp b/src/motors/HybridStepperMotorOld/HybridStepperMotorOld.cpp similarity index 95% rename from src/motors/HybridStepperMotor/HybridStepperMotor.cpp rename to src/motors/HybridStepperMotorOld/HybridStepperMotorOld.cpp index 5755ad8..aba890b 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp +++ b/src/motors/HybridStepperMotorOld/HybridStepperMotorOld.cpp @@ -1,12 +1,12 @@ -#include "HybridStepperMotor.h" +#include "HybridStepperMotorOld.h" #include "./communication/SimpleFOCDebug.h" -// HybridStepperMotor(int pp) +// HybridStepperMotorOld(int pp) // - pp - pole pair number // - R - motor phase resistance // - KV - motor kv rating (rmp/v) // - L - motor phase inductance [H] -HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) +HybridStepperMotorOld::HybridStepperMotorOld(int pp, float _R, float _KV, float _inductance) : FOCMotor() { // number od pole pairs @@ -27,13 +27,13 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc /** Link the driver which controls the motor */ -void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +void HybridStepperMotorOld::linkDriver(BLDCDriver *_driver) { driver = _driver; } // init hardware pins -int HybridStepperMotor::init() +int HybridStepperMotorOld::init() { if (!driver || !driver->initialized) { @@ -74,7 +74,7 @@ int HybridStepperMotor::init() } // disable motor driver -void HybridStepperMotor::disable() +void HybridStepperMotorOld::disable() { // set zero to PWM driver->setPwm(0, 0, 0); @@ -84,7 +84,7 @@ void HybridStepperMotor::disable() enabled = 0; } // enable motor driver -void HybridStepperMotor::enable() +void HybridStepperMotorOld::enable() { // disable enable driver->enable(); @@ -97,7 +97,7 @@ void HybridStepperMotor::enable() /** * FOC functions */ -int HybridStepperMotor::initFOC() +int HybridStepperMotorOld::initFOC() { int exit_flag = 1; @@ -132,7 +132,7 @@ int HybridStepperMotor::initFOC() } // Encoder alignment to electrical 0 angle -int HybridStepperMotor::alignSensor() +int HybridStepperMotorOld::alignSensor() { int exit_flag = 1; // success SIMPLEFOC_DEBUG("MOT: Align sensor."); @@ -226,7 +226,7 @@ int HybridStepperMotor::alignSensor() // Encoder alignment the absolute zero angle // - to the index -int HybridStepperMotor::absoluteZeroSearch() +int HybridStepperMotorOld::absoluteZeroSearch() { SIMPLEFOC_DEBUG("MOT: Index search..."); @@ -261,7 +261,7 @@ int HybridStepperMotor::absoluteZeroSearch() // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better -void HybridStepperMotor::loopFOC() +void HybridStepperMotorOld::loopFOC() { // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track @@ -293,7 +293,7 @@ void HybridStepperMotor::loopFOC() // It runs either angle, velocity or voltage loop // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value -void HybridStepperMotor::move(float new_target) +void HybridStepperMotorOld::move(float new_target) { // set internal target variable @@ -394,7 +394,7 @@ void HybridStepperMotor::move(float new_target) } } -void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +void HybridStepperMotorOld::setPhaseVoltage(float Uq, float Ud, float angle_el) { angle_el = _normalizeAngle(angle_el); float _ca = _cos(angle_el); @@ -473,7 +473,7 @@ void HybridStepperMotor::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 -float HybridStepperMotor::velocityOpenloop(float target_velocity) +float HybridStepperMotorOld::velocityOpenloop(float target_velocity) { // get current timestamp unsigned long now_us = _micros(); @@ -509,7 +509,7 @@ float HybridStepperMotor::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 -float HybridStepperMotor::angleOpenloop(float target_angle) +float HybridStepperMotorOld::angleOpenloop(float target_angle) { // get current timestamp unsigned long now_us = _micros(); diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.h b/src/motors/HybridStepperMotorOld/HybridStepperMotorOld.h similarity index 90% rename from src/motors/HybridStepperMotor/HybridStepperMotor.h rename to src/motors/HybridStepperMotorOld/HybridStepperMotorOld.h index f235ee6..0d669a3 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.h +++ b/src/motors/HybridStepperMotorOld/HybridStepperMotorOld.h @@ -1,10 +1,10 @@ /** - * @file HybridStepperMotor.h + * @file HybridStepperMotorOld.h * */ -#ifndef HybridStepperMotor_h -#define HybridStepperMotor_h +#ifndef HybridStepperMotorOld_h +#define HybridStepperMotorOld_h #include "Arduino.h" #include "common/base_classes/FOCMotor.h" @@ -17,17 +17,17 @@ /** Stepper Motor class */ -class HybridStepperMotor : public FOCMotor +class HybridStepperMotorOld : public FOCMotor { public: /** - HybridStepperMotor class constructor + HybridStepperMotorOld class constructor @param pp pole pair number @param R motor phase resistance - [Ohm] @param KV motor KV rating (1/K_bemf) - rpm/V @param L motor phase inductance - [H] */ - HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + HybridStepperMotorOld(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); /** * Function linking a motor and a foc driver @@ -63,7 +63,7 @@ class HybridStepperMotor : public FOCMotor void loopFOC() override; /** - * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * Function executing the control loops set by the controller parameter of the HybridStepperMotorOld. * * @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 diff --git a/src/motors/HybridStepperMotor/README.md b/src/motors/HybridStepperMotorOld/README.md similarity index 100% rename from src/motors/HybridStepperMotor/README.md rename to src/motors/HybridStepperMotorOld/README.md diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp index 24ebadb..9ef2886 100644 --- a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -86,8 +86,12 @@ void CAT24I2CFlashSettingsStorage::reset() { int CAT24I2CFlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { int read = _wire->requestFrom((uint8_t)_address, (uint8_t)numBytes, (uint8_t)true); _currptr += read; - if (read==numBytes) - _wire->readBytes((uint8_t*)valueToSet, numBytes); + if (read==numBytes) { + //_wire->readBytes((uint8_t*)valueToSet, numBytes); + for (int i=0; i < numBytes; i++) { + ((uint8_t*)valueToSet)[i] = _wire->read(); + } + } return read; }; diff --git a/src/utilities/stm32pwm/STM32PWMInput.cpp b/src/utilities/stm32pwm/STM32PWMInput.cpp index c530c04..d6611b0 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.cpp +++ b/src/utilities/stm32pwm/STM32PWMInput.cpp @@ -1,12 +1,15 @@ #include "./STM32PWMInput.h" +#include +#include "communication/SimpleFOCDebug.h" #if defined(_STM32_DEF_) -STM32PWMInput::STM32PWMInput(int pin){ +STM32PWMInput::STM32PWMInput(int pin, uint32_t pwm_freq){ _pin = digitalPinToPinName(pin); + _pwm_freq = pwm_freq; }; @@ -20,9 +23,13 @@ int STM32PWMInput::initialize(){ pinmap_pinout(_pin, PinMap_TIM); uint32_t channel = STM_PIN_CHANNEL(pinmap_function(_pin, PinMap_TIM)); timer.Instance = (TIM_TypeDef *)pinmap_peripheral(_pin, PinMap_TIM); - timer.Init.Prescaler = 0; timer.Init.CounterMode = TIM_COUNTERMODE_UP; - timer.Init.Period = 4.294967295E9; // TODO max period, depends on which timer is used - 32 bits or 16 bits + // Check if timer is 16 or 32 bit and set max period accordingly + if (IS_TIM_32B_COUNTER_INSTANCE(timer.Instance)) { + timer.Init.Period = 0xFFFFFFFF; // 32-bit timer max + } else { + timer.Init.Period = 0xFFFF; // 16-bit timer max + } timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; timer.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (channel!=1 && channel!=2) // only channels 1 & 2 supported @@ -76,6 +83,32 @@ int STM32PWMInput::initialize(){ if (HAL_TIM_IC_Start(&timer, TIM_CHANNEL_2)!=HAL_OK) { return -9; } + + // Check if the timer period is longer than the PWM period + // if it isnt set the perescaler to make it longer + // Calculate timer clock frequency + uint32_t timer_clk = HAL_RCC_GetPCLK1Freq(); + if (IS_TIM_CLOCK_DIVISION_INSTANCE(timer.Instance)) { + // If APB1 prescaler > 1, timer clock is doubled + if ((RCC->CFGR & RCC_CFGR_PPRE1) != RCC_CFGR_PPRE1_DIV1) { + timer_clk *= 2; + } + } + + // Calculate required period (in timer ticks) for one PWM period + uint32_t desired_period_ticks = timer_clk / _pwm_freq; + + // Check if timer's max period can fit the desired period + uint32_t max_period = (IS_TIM_32B_COUNTER_INSTANCE(timer.Instance)) ? 0xFFFFFFFF : 0xFFFF; + uint32_t prescaler = 1; + if (desired_period_ticks > max_period) { + prescaler = (desired_period_ticks + max_period - 1) / max_period; + if (prescaler > 0xFFFF) prescaler = 0xFFFF; // limit to 16-bit prescaler + } + + // Set the prescaler to achieve the desired period + LL_TIM_SetPrescaler(timer.Instance, prescaler); + timer.Instance->CR1 |= TIM_CR1_CEN; return 0; }; diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h index fcf3acd..9b6fb0d 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.h +++ b/src/utilities/stm32pwm/STM32PWMInput.h @@ -7,19 +7,48 @@ class STM32PWMInput { public: - STM32PWMInput(int pin); + /** + * StM32PWMInput constructor + * + * @param pin - the pin number to read the PWM signal from + * @param pwm_freq - the frequency of the PWM signal (default is 1kHz) + * + * This class is used to read PWM signals from a pin using the STM32 HAL library. + * IMPORTANT! + * This class can only be used with the pins that are assocaited with some timer, + * and only if they are associated to the channels 1 or 2. The timer can not be + * used for other purposes, like motor control. + */ + STM32PWMInput(int pin, uint32_t pwm_freq = 1000); ~STM32PWMInput(); int initialize(); + /** + * Get the duty cycle of the PWM signal as a percentage. + * + * @return float - the duty cycle in percent + */ float getDutyCyclePercent(); + /** + * Get the duty cycle of the PWM signal in ticks. + * + * @return uint32_t - the duty cycle in ticks + */ uint32_t getDutyCycleTicks(); + /** + * Get the period of the PWM signal in ticks. + * + * @return uint32_t - the period in ticks + */ uint32_t getPeriodTicks(); - PinName _pin; + PinName _pin; // the pin to read the PWM signal from + uint32_t _pwm_freq; // the frequency of the PWM signal + protected: - TIM_HandleTypeDef timer; - bool useChannel2 = false; + TIM_HandleTypeDef timer; // the timer handle for the PWM input + bool useChannel2 = false; // whether to use channel 2 or not, default is channel 1 }; diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index 714a0cb..2b1a065 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -18,7 +18,7 @@ GenericVoltageSense::GenericVoltageSense(int pin, float gain, float offset, floa bool GenericVoltageSense::init(int resolution){ pinMode(pin, INPUT_ANALOG); -#if !defined(ARDUINO_ARCH_AVR) && !defined(ARDUINO_ARCH_MEGAAVR) +#if !defined(ARDUINO_ARCH_AVR) && !defined(ARDUINO_ARCH_MEGAAVR) && !defined(ARDUINO_ARCH_SILABS) if (resolution>0) { analogReadResolution(resolution); maxValue = _powtwo(resolution);