diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml new file mode 100644 index 00000000..cadd8ccc --- /dev/null +++ b/.github/workflows/arduino.yml @@ -0,0 +1,53 @@ +name: AVR +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:avr:mega # arduino mega2650 + - arduino:avr:leonardo # arduino leonardo + + include: + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples + sketch-names: '**.ino' + required-libraries: PciManager + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, 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, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example + sketch-names: single_full_control_example.ino + + + # 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@master + 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 }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml deleted file mode 100644 index 59ebbcf2..00000000 --- a/.github/workflows/ccpp.yml +++ /dev/null @@ -1,101 +0,0 @@ -name: Library Compile -on: [push, pull_request] -jobs: - build: - name: Test compiling - runs-on: ubuntu-latest - - strategy: - matrix: - arduino-boards-fqbn: - - arduino:avr:uno # arudino uno - - arduino:sam:arduino_due_x # arduino due - - arduino:avr:mega # arduino mega2650 - - arduino:avr:leonardo # arduino leonardo - - arduino:samd:nano_33_iot # samd21 - - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esp32 - - esp32:esp32:esp32s2 # esp32s2 - - esp32:esp32:esp32s3 # esp32s3 - - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - - arduino:mbed_rp2040:pico # rpi pico - - include: - - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples - sketch-names: '**.ino' - required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, 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, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - - - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example - sketch-names: single_full_control_example.ino - - - 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 # raspberry pi pico - one example - sketch-names: open_loop_position_example.ino - - - 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: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:esp32s3 # esp32s3 - 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 - - - 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 - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: B_G431B_ESC1.ino - build-properties: - B_G431B_ESC1: - -DHAL_OPAMP_MODULE_ENABLED - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - - - # 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@master - 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 }} - build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml new file mode 100644 index 00000000..db551b41 --- /dev/null +++ b/.github/workflows/esp32.yml @@ -0,0 +1,55 @@ +name: ESP32 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - esp32:esp32:esp32doit-devkit-v1 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 + - esp32:esp32:esp32c3 # esp32c3 + + include: + + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino + + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + + - arduino-boards-fqbn: esp32:esp32:esp32c3 # esp32c3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_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 + + # 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@master + 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 }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml new file mode 100644 index 00000000..d6d72b95 --- /dev/null +++ b/.github/workflows/rpi.yml @@ -0,0 +1,39 @@ +name: RP2040 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:mbed_rp2040:pico # rpi pico + + include: + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example + sketch-names: open_loop_position_example.ino + + # 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@master + 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 }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml new file mode 100644 index 00000000..c4329869 --- /dev/null +++ b/.github/workflows/samd.yml @@ -0,0 +1,44 @@ +name: SAMD +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + + include: + + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - 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 + + # 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@master + 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 }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/stm32.yml b/.github/workflows/stm32.yml new file mode 100644 index 00000000..52b5cc94 --- /dev/null +++ b/.github/workflows/stm32.yml @@ -0,0 +1,68 @@ +name: STM32 +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg + - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 + - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + + include: + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: B_G431B_ESC1.ino + build-properties: + B_G431B_ESC1: + -DHAL_OPAMP_MODULE_ENABLED + + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + + + # 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@master + 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 }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml new file mode 100644 index 00000000..6ad953fe --- /dev/null +++ b/.github/workflows/teensy.yml @@ -0,0 +1,46 @@ +name: Teensy + +on: [push] + +jobs: + build: + + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/cache@v3 + with: + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio + - uses: actions/setup-python@v4 + with: + python-version: '3.9' + - name: Install PlatformIO Core + run: pip install --upgrade platformio + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino + + - name: PIO Run Teensy 3 + run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino + + - name: PIO Run Teensy 3 + run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino diff --git a/README.md b/README.md index 424f3cd7..4fbb7517 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,20 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) +[![AVR build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml) +[![STM32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml) +[![ESP32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml) +[![RP2040 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml) +[![SAMD build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml) +[![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) + ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) @@ -18,40 +25,31 @@ 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 FOC supporting BLDC driver boards: - - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini). - - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - *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.3.0 -> - Arduino Mega 6pwm more timers supported -> - Arduino boards - frequency change support either 32kHz or 4kHz -> - Arduino Uno - synched timers in 3pwm and 6pwm mode [#71](https://github.com/simplefoc/Arduino-FOC/issues/71) -> - Teensy 3.x initial support for 6pwm -> - Teensy 4.x initial support for 6pwm -> - Example for v3.1 SimpleFOCShield -> - RP2040 compatibility for earlehillpower core [#234](https://github.com/simplefoc/Arduino-FOC/pull/234) [#236](https://github.com/simplefoc/Arduino-FOC/pull/236) -> - More flexible monitoring API -> - start, end and separator characters -> - decimal places (settable through commander) -> - Added machine readable verbose mode in `Commander` [#233](https://github.com/simplefoc/Arduino-FOC/pull/233) -> - *Simple**FOC**WebController* - Web based user interface for SimpleFOC by [@geekuillaume](https://github.com/geekuillaume) - [webcontroller.simplefoc.com](webcontroller.simplefoc.com) -> - bugfix - `MagneticSensorPWM` multiple occasions - [#258](https://github.com/simplefoc/Arduino-FOC/pull/258) -> - bugfix - current sense align - added offset exchange when exchanging pins -> - bugfix - trapezoid 150 fixed -> - bugfix - 4pwm on ESP8266 [#224](https://github.com/simplefoc/Arduino-FOC/pull/224) -> - Additional `InlineCurrentSense` and `LowsideCurrentSense` constructor using milliVolts per Amp [#253](https://github.com/simplefoc/Arduino-FOC/pull/253) -> - STM32L4xx current sense support by [@Triple6](https://github.com/Triple6) (discord) [#257](https://github.com/simplefoc/Arduino-FOC/pull/257) -> - phase disable in 6pwm mode -> - stm32 - software and hardware 6pwm -> - atmega328 -> - atmega2560 -> - Lag compensation using motor inductance [#246](https://github.com/simplefoc/Arduino-FOC/issues/246) -> - current control through voltage torque mode enhancement -> - extended `BLDCMotor` and `StepperMotor` constructors to receive the inductance paramerer -> - can also be set using `motor.phase_inductance` or through `Commander` -## Arduino *SimpleFOClibrary* v2.3 +- 🎯 Develop modular and easy to use FOC supporting BLDC driver boards + - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) + - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) + +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 +> - ESP32 MCUs extended support [#414](https://github.com/simplefoc/Arduino-FOC/pull/414) +> - Transition to the arduino-esp32 version v3.x (ESP-IDF v5.x) [#387](https://github.com/espressif/arduino-esp32/releases) +> - New support for MCPWM driver +> - New support for LEDC drivers - center-aligned PWM and 6PWM available +> - Rewritten and simplified the fast ADC driver code (`adcRead`) - for low-side and inline current sensing. +> - Stepper motors current sensing support [#421](https://github.com/simplefoc/Arduino-FOC/pull/421) +> - Support for current sensing (low-side and inline) - [see in docs](https://docs.simplefoc.com/current_sense) +> - Support for true FOC control - `foc_current` torque control - [see in docs](https://docs.simplefoc.com/motion_control) +> - New current sense alignment procedure [#422](https://github.com/simplefoc/Arduino-FOC/pull/422) - [see in docs](https://docs.simplefoc.com/current_sense_align) +> - Support for steppers +> - Much more robust and reliable +> - More verbose and informative +> - Support for HallSensors without interrupts [#424](https://docs.simplefoc.com/https://github.com/simplefoc/Arduino-FOC/pull/424) - [see in docs](hall_sensors) +> - Docs +> - A short guide to debugging of common issues +> - A short guide to the units in the library - [see in docs](https://docs.simplefoc.com/library_units) +> - See the complete list of bugfixes and new features of v2.3.4 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/11) + + +## Arduino *SimpleFOClibrary* v2.3.4

@@ -78,7 +76,7 @@ This video demonstrates the *Simple**FOC**library* basic usage, electronic conne - **Cross-platform**: - Seamless code transfer from one microcontroller family to another - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers): - - Arduino: UNO, MEGA, DUE, Leonardo .... + - Arduino: UNO R4, UNO, MEGA, DUE, Leonardo, Nano, Nano33 .... - STM32 - ESP32 - Teensy @@ -132,7 +130,9 @@ Please do not hesitate to leave an issue if you have problems/advices/suggestion Pull requests are welcome, but let's first discuss them in [community forum](https://community.simplefoc.com)! -If you'd like to contribute to this porject but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. +If you'd like to contribute to this project but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. + +If you are familiar, we accept pull requests to the dev branch! ## Arduino code example This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder. 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 51ac990e..f1eeefd2 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,6 +7,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL); +// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21 LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT); @@ -25,6 +26,12 @@ void doTarget(char* cmd) { command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -75,9 +82,6 @@ void setup() { // 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); @@ -96,6 +100,7 @@ void setup() { void loop() { // main FOC algorithm function + motor.loopFOC(); // Motion control function motor.move(); @@ -106,4 +111,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} 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 be853f0e..b29e45d8 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 @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB, doI); @@ -75,8 +81,6 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 e27f1e7a..caef7ffe 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 @@ -36,6 +36,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -72,8 +78,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 842f383a..3d90db16 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 @@ -43,6 +43,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -90,9 +96,6 @@ void setup() { // 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); 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 a5b72f82..af56e067 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 @@ -29,7 +29,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); -BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE); +BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INL_A, INH_B,INL_B, INH_C,INL_C, EN_GATE); // encoder instance Encoder encoder = Encoder(2, 3, 8192); @@ -46,6 +46,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -91,9 +97,6 @@ void setup() { // 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); 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 84033b42..7d7fe14f 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 @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { 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 diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino index 3e3f04ab..e1b4c392 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { 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 diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino new file mode 100644 index 00000000..c9b4516f --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -0,0 +1,170 @@ +/** + * 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 EN_GATE 11 +#define M_PWM 22 +#define GAIN 20 +#define M_OC 23 +#define OC_ADJ 19 + +#define INH_A 2 +#define INL_A 3 +#define INH_B 8 +#define INL_B 7 +#define INH_C 6 +#define INL_C 9 + +#define IOUTA 14 +#define IOUTB 15 +#define IOUTC 16 + +// 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); + +// encoder instance +Encoder encoder = Encoder(10, 11, 2048); + +// 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() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // 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 6pwm mode + pinMode(M_PWM, OUTPUT); + digitalWrite(M_PWM,LOW); // high for 3pwm + // 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 = 19; + driver.pwm_frequency = 20000; // suggested not higher than 22khz + 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 + 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 + + // 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 = 0; + + // initialise motor + motor.init(); + + cs.init(); + // 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 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 f6dac940..7f6b33ce 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 @@ -24,6 +24,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -66,9 +72,6 @@ void setup() { // 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); 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 f025895b..9ba9604a 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 @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -69,8 +75,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 5b5c8e40..d9107654 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 @@ -42,6 +42,9 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // initialise encoder hardware encoder.init(); // interrupt initialization @@ -85,9 +88,6 @@ void setup() { // 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); diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino index b89c215f..504ab9c8 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -70,6 +70,12 @@ void doI(){encoder.handleIndex();} void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -96,8 +102,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino index 2aed6bfb..7abcde54 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -69,6 +69,12 @@ SPIClass SPI_3(SPI3_MOSO, SPI3_MISO, SPI3_SCL); void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -94,8 +100,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; 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 70ec28b0..650050ef 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 @@ -23,7 +23,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(1000); Serial.println("Initializing..."); 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 eb52f51b..bc387437 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 @@ -21,6 +21,12 @@ void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -56,9 +62,6 @@ void setup() { // 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 diff --git a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino index 9102c89a..8eb122a0 100644 --- a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -38,6 +38,12 @@ Commander command = Commander(Serial); void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // if SimpleFOCMini is stacked in arduino headers // on pins 12,11,10,9,8 // pin 12 is used as ground @@ -84,9 +90,6 @@ void setup() { // 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); diff --git a/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino new file mode 100644 index 00000000..c458718b --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino @@ -0,0 +1,93 @@ +/** + * + * SimpleFOCMini motor control example + * + * For Arduino UNO or the other boards with the UNO headers + * the most convenient way to use the board is to stack it to the pins: + * - 12 - ENABLE + * - 11 - IN1 + * - 10 - IN2 + * - 9 - IN3 + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0 +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 12); // mini v1.1 + +// instantiate the commander +Commander command = Commander(Serial); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if SimpleFOCMini is stacked in arduino headers + // on pins 12,11,10,9,8 + // pin 12 is used as ground + pinMode(12,OUTPUT); + pinMode(12,LOW); + + // 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; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity_openloop; + + // default voltage_power_supply + motor.voltage_limit = 2; // Volts + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "motor"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + + motor.target = 1; //initial target velocity 1 rad/s + Serial.println("Target velocity: 1 rad/s"); + Serial.println("Voltage limit 2V"); + _delay(1000); +} + +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(); + + // 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/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 c048956b..fb5e1563 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 @@ -28,6 +28,12 @@ void doMotor2(char* cmd){ command.motor(&motor2, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -72,9 +78,6 @@ void setup() { 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); 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 10e26c9f..3faa38b2 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 @@ -17,6 +17,12 @@ void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -48,9 +54,6 @@ void setup() { // 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 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 b40ab62a..8eb3a86d 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 @@ -39,6 +39,12 @@ void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -88,9 +94,6 @@ void setup() { 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); 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 76a7296a..b65a9cb0 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 @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // 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 diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 02593286..6359f201 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -13,7 +13,7 @@ void doB(){encoder.handleB();} // inline current sensor instance // ACS712-05B has the resolution of 0.185mV per Amp -InlineCurrentSense current_sense = InlineCurrentSense(1.0f, 0.185f, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); // commander communication instance Commander command = Commander(Serial); @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // 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 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 index 80f2fe64..18047108 100644 --- a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -18,6 +18,12 @@ void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&SerialUSB); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -31,8 +37,6 @@ void setup() { // 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); diff --git a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 40924ee3..9143dde7 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -18,6 +18,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -48,7 +54,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 9c6eea03..5ff53311 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -33,6 +33,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -63,7 +69,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); 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 83b1fb32..c1ff7bbc 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,6 +23,12 @@ void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -30,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -46,14 +55,16 @@ void setup() { motor.controller = MotionControlType::angle_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // 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!"); Serial.println("Set target position [rad]"); _delay(1000); @@ -62,6 +73,7 @@ void setup() { void loop() { // open loop angle movements // using motor.voltage_limit and motor.velocity_limit + // angles can be positive or negative, negative angles correspond to opposite motor direction motor.move(target_position); // user communication 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 84f87ab9..b1bc2760 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,6 +23,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -30,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -44,13 +53,15 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); @@ -60,6 +71,7 @@ void loop() { // open loop velocity movement // using motor.voltage_limit and motor.velocity_limit + // to turn the motor "backwards", just set a negative target_velocity motor.move(target_velocity); // user communication 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 ccb3980e..03e6ea75 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 @@ -48,6 +48,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -88,9 +94,6 @@ void setup() { // 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); 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 752030c9..f0f9b27c 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 @@ -31,6 +31,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -67,9 +73,7 @@ void setup() { motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); 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 c9123401..82aec86a 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 @@ -27,6 +27,12 @@ void doTarget(char* cmd) { command.scalar(&target_current, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -70,8 +76,6 @@ void setup() { // motor.LPF_current_q.Tf = 0.002f; // 1ms default // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 66ff4569..219637c9 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 @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,8 +61,6 @@ void setup() { // 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); 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 7273d156..c72c9224 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 @@ -35,6 +35,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -57,8 +63,6 @@ void setup() { // 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); 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 d869f4dd..dbb56080 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 @@ -30,6 +30,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,8 +53,6 @@ void setup() { // 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); 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/velocity_control.ino similarity index 96% rename from examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino rename to examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino index 1d39173a..4fb47f02 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino @@ -47,6 +47,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize sensor sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -84,8 +90,6 @@ void setup() { // velocity low pass filtering time constant motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 98e09425..40299b8f 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 @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,8 +72,6 @@ void setup() { // the lower the less filtered motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); 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 186d257e..d6ddc6b4 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 @@ -34,6 +34,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -68,9 +74,6 @@ void setup() { // 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); 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 df7b76ac..ff6fe7f3 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,6 +41,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -76,9 +82,6 @@ void setup() { // 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); 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 098f6888..e863d8c9 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 @@ -33,6 +33,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,9 +72,6 @@ void setup() { // 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); 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 afb95294..14d4867c 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 @@ -67,8 +67,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); Commander command = Commander(Serial); void setup() { + // use monitoring with serial Serial.begin(115200); - + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + WiFi.begin(ssid, pass); Serial.print("Connecting WiFi "); 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 7c6aa4f0..940b59c2 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 @@ -104,7 +104,12 @@ void setup() { digitalWrite(26, 0); digitalWrite(27, 0); + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(200); WiFi.begin(ssid, pass); 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 7324edf0..9c523c22 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 @@ -80,8 +80,11 @@ void testAlignmentAndCogging(int direction) { void setup() { + // use monitoring with serial Serial.begin(115200); - while (!Serial) ; + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); // driver config driver.voltage_power_supply = 12; diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index 76fb46b0..c39657b1 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -36,12 +36,18 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); @@ -62,8 +68,6 @@ void setup() { // 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); diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 3abef467..4eadd376 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -33,12 +33,18 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -58,9 +64,7 @@ void setup() { // 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); diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index f3dd74a1..1df631af 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -31,12 +31,18 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); // link the motor to the sensor @@ -56,8 +62,6 @@ void setup() { // 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); 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 aac879cd..ad8e69ce 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 @@ -32,6 +32,12 @@ void doB(){encoder.handleB();} void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -48,8 +54,6 @@ void setup() { // initialize motor motor.init(); - // monitoring port - Serial.begin(115200); // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); 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 e98a4529..b44bc0bb 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 @@ -33,6 +33,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,9 +53,6 @@ void setup() { // initialize motor hardware motor.init(); - // monitoring port - Serial.begin(115200); - // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); Serial.println("-\n"); 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 3ad87cca..407469fc 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 @@ -2,8 +2,9 @@ * Simple example intended to help users find the zero offset and natural direction of the sensor. * * These values can further be used to avoid motor and sensor alignment procedure. - * - * motor.initFOC(zero_offset, sensor_direction); + * To use these values add them to the code:"); + * motor.sensor_direction=Direction::CW; // or Direction::CCW + * motor.zero_electric_angle=1.2345; // use the real value! * * This will only work for abosolute value sensors - magnetic sensors. * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors. @@ -28,6 +29,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // power supply voltage driver.voltage_power_supply = 12; driver.init(); @@ -44,19 +51,28 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; + // force direction search - because default is CW + motor.sensor_direction = Direction::UNKNOWN; + // initialize motor motor.init(); // align sensor and start FOC motor.initFOC(); - Serial.begin(115200); Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); - Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW"); + Serial.println(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + + Serial.println("To use these values add them to the code:"); + Serial.print(" motor.sensor_direction="); + Serial.print(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + Serial.println(";"); + Serial.print(" motor.zero_electric_angle="); + Serial.print(motor.zero_electric_angle, 4); + Serial.println(";"); - Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)"); _delay(1000); Serial.println("If motor is not moving the alignment procedure was not successfull!!"); } 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 cbeb6ecb..074538ad 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 @@ -29,6 +29,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -46,7 +52,6 @@ void setup() { motor.controller = MotionControlType::torque; // use monitoring with serial - Serial.begin(115200); motor.useMonitoring(Serial); // initialize motor motor.init(); 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 f8978577..4cd87970 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 @@ -24,6 +24,12 @@ void onStep() { step_dir.handle(); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -64,9 +70,7 @@ void setup() { 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); 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 index e999de23..2448a51c 100644 --- 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 @@ -31,6 +31,14 @@ GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCu void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if callbacks are not provided in the constructor // they can be assigned directly: //current_sense.readCallback = readCurrentSense; @@ -40,7 +48,6 @@ void setup() { current_sense.init(); - Serial.begin(115200); Serial.println("Current sense ready."); } 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 c1a5139b..1198cdcd 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 @@ -11,13 +11,22 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise the current sensing - current_sense.init(); + if(!current_sense.init()){ + Serial.println("Current sense init failed."); + return; + } // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/driver_standalone_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 index 1235fccd..eef793d7 100644 --- a/examples/utils/driver_standalone_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 @@ -7,6 +7,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -17,11 +23,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _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 478d3974..56a1afbe 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 @@ -6,6 +6,12 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -18,11 +24,14 @@ void setup() { driver.dead_zone = 0.05f; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } 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 4627efa9..59343a14 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 @@ -13,6 +13,12 @@ StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -23,11 +29,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } 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 e028a737..a58d7940 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 @@ -8,6 +8,12 @@ StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -18,11 +24,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } 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 c4777baa..cc8dfdb8 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 @@ -13,13 +13,6 @@ // - pp - pole pairs HallSensor sensor = HallSensor(2, 3, 4, 14); -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){sensor.handleA();} -void doB(){sensor.handleB();} -void doC(){sensor.handleC();} - - void setup() { // monitoring port Serial.begin(115200); @@ -29,8 +22,6 @@ void setup() { // initialise encoder hardware sensor.init(); - // hardware interrupt enable - sensor.enableInterrupts(doA, doB, doC); Serial.println("Sensor ready"); _delay(1000); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino new file mode 100644 index 00000000..c4777baa --- /dev/null +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino @@ -0,0 +1,48 @@ +/** + * Hall sensor example code + * + * This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup. + * + */ + +#include + +// Hall sensor instance +// HallSensor(int hallA, int hallB , int cpr, int index) +// - hallA, hallB, hallC - HallSensor A, B and C pins +// - pp - pole pairs +HallSensor sensor = HallSensor(2, 3, 4, 14); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} + + +void setup() { + // monitoring port + Serial.begin(115200); + + // check if you need internal pullups + sensor.pullup = Pullup::USE_EXTERN; + + // initialise encoder hardware + sensor.init(); + // hardware interrupt enable + sensor.enableInterrupts(doA, doB, doC); + + 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()); + delay(100); +} diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensor_software_interrupts_example.ino similarity index 100% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensor_software_interrupts_example.ino diff --git a/keywords.txt b/keywords.txt index 1f200e52..d8fd3e73 100644 --- a/keywords.txt +++ b/keywords.txt @@ -133,6 +133,7 @@ sensor_direction KEYWORD2 sensor_offset KEYWORD2 zero_electric_angle KEYWORD2 verbose KEYWORD2 +verboseMode KEYWORD1 decimal_places KEYWORD2 phase_resistance KEYWORD2 phase_inductance KEYWORD2 diff --git a/library.json b/library.json new file mode 100644 index 00000000..25e2a353 --- /dev/null +++ b/library.json @@ -0,0 +1,5 @@ +{ + "build": { + "libArchive": false + } +} diff --git a/library.properties b/library.properties index 401523ba..5daa49d9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.0 +version=2.3.4 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fce552c6..501a8bc9 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -44,7 +44,9 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/KV // 1/sqrt(2) - rms value - KV_rating = _KV*_SQRT2; + KV_rating = NOT_SET; + if (_isset(_KV)) + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -61,11 +63,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { } // init hardware pins -void BLDCMotor::init() { +int BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -90,18 +92,28 @@ void BLDCMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } // disable motor driver void BLDCMotor::disable() { + // disable the current sense + if(current_sense) current_sense->disable(); // set zero to PWM driver->setPwm(0, 0, 0); // disable the driver @@ -116,6 +128,13 @@ void BLDCMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); + // enable the current sense + if(current_sense) current_sense->enable(); + // reset the pids + PID_velocity.reset(); + P_angle.reset(); + PID_current_q.reset(); + PID_current_d.reset(); // motor status update enabled = 1; } @@ -124,48 +143,45 @@ void BLDCMotor::enable() FOC functions */ // FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { +int BLDCMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - 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; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else { - exit_flag = 0; // no FOC without sensor - SIMPLEFOC_DEBUG("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(exit_flag){ - if(current_sense){ - if (!current_sense->initialized) { - motor_status = FOCMotorStatus::motor_calib_failed; - SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); - exit_flag = 0; - }else{ - exit_flag *= alignCurrentSense(); + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor } - else SIMPLEFOC_DEBUG("MOT: No current sense."); } if(exit_flag){ @@ -187,7 +203,7 @@ int BLDCMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); @@ -210,14 +226,18 @@ int BLDCMotor::alignSensor() { // stop init if not found index if(!exit_flag) return exit_flag; + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction - if(!_isset(sensor_direction)){ + if(sensor_direction==Direction::UNKNOWN){ // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -227,13 +247,13 @@ int BLDCMotor::alignSensor() { // 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); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } sensor->update(); float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); + // setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved float moved = fabs(mid_angle - end_angle); @@ -248,18 +268,20 @@ int BLDCMotor::alignSensor() { sensor_direction = Direction::CW; } // check pole pair number - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { SIMPLEFOC_DEBUG("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); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); @@ -274,7 +296,7 @@ int BLDCMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -303,8 +325,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + if(sensor->needsSearch()) { SIMPLEFOC_DEBUG("MOT: Error: Not found!"); } + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -372,6 +394,9 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target)) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -389,11 +414,9 @@ void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; @@ -418,7 +441,8 @@ void BLDCMotor::move(float new_target) { // angle set point shaft_angle_sp = target; // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); // 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 @@ -541,119 +565,43 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { break; case FOCModulationType::SinePWM : + case FOCModulationType::SpaceVectorPWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; - // center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - center = driver->voltage_limit/2; // Clarke transform - Ua = Ualpha + center; - Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; + Ua = Ualpha; + Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta; + Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta; + + center = driver->voltage_limit/2; + if (foc_modulation == FOCModulationType::SpaceVectorPWM){ + // discussed here: https://community.simplefoc.com/t/embedded-world-2023-stm32-cordic-co-processor/3107/165?u=candas1 + // a bit more info here: https://microchipdeveloper.com/mct5001:which-zsm-is-best + // Midpoint Clamp + float Umin = min(Ua, min(Ub, Uc)); + float Umax = max(Ua, max(Ub, Uc)); + center -= (Umax+Umin) / 2; + } if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); Ua -= Umin; Ub -= Umin; Uc -= Umin; + }else{ + Ua += center; + Ub += center; + Uc += center; } break; - case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm - // https://www.youtube.com/watch?v=QMSWUMEAejg - - // 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) - - 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 - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * 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) { - T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 - } - - // calculate the duty cycles(times) - float Ta,Tb,Tc; - switch(sector){ - case 1: - Ta = T1 + T2 + T0/2; - Tb = T2 + T0/2; - Tc = T0/2; - break; - case 2: - Ta = T1 + T0/2; - Tb = T1 + T2 + T0/2; - Tc = T0/2; - break; - case 3: - Ta = T0/2; - Tb = T1 + T2 + T0/2; - Tc = T2 + T0/2; - break; - case 4: - Ta = T0/2; - Tb = T1+ T0/2; - Tc = T1 + T2 + T0/2; - break; - case 5: - Ta = T2 + T0/2; - Tb = T0/2; - Tc = T1 + T2 + T0/2; - break; - case 6: - Ta = T1 + T2 + T0/2; - Tb = T0/2; - Tc = T1 + T0/2; - break; - default: - // possible error state - Ta = 0; - Tb = 0; - Tc = 0; - } - - // calculate the phase voltages and center - Ua = Ta*driver->voltage_limit; - Ub = Tb*driver->voltage_limit; - Uc = Tc*driver->voltage_limit; - break; - } // set the voltages in driver diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 366f4600..c261e405 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" +#include "common/base_classes/FOCDriver.h" #include "common/base_classes/BLDCDriver.h" #include "common/foc_utils.h" #include "common/time_utils.h" @@ -39,7 +40,7 @@ class BLDCMotor: public FOCMotor BLDCDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -49,7 +50,7 @@ class BLDCMotor: public FOCMotor * Function initializing FOC algorithm * and aligning sensor's and motors' zero position */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -69,8 +70,7 @@ 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 - + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8865f57c..1d8f3147 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -16,7 +16,7 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/K_bemf // usually used rms - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -33,11 +33,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { } // init hardware pins -void StepperMotor::init() { +int StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -56,6 +56,13 @@ void StepperMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); @@ -63,6 +70,7 @@ void StepperMotor::init() { _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } @@ -92,29 +100,46 @@ void StepperMotor::enable() FOC functions */ // FOC initialization function -int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { +int StepperMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - 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; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); - shaft_angle = sensor->getAngle(); - }else SIMPLEFOC_DEBUG("MOT: No sensor."); + shaft_angle = shaftAngle(); + + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -128,13 +153,37 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct return exit_flag; } +// Calibrate the motor and current sense phases +int StepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success SIMPLEFOC_DEBUG("MOT: Align sensor."); + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction - if(!_isset(sensor_direction)){ + if(sensor_direction == Direction::UNKNOWN){ // check if sensor needs zero search if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); // stop init if not found index @@ -144,7 +193,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -154,7 +203,7 @@ int StepperMotor::alignSensor() { // 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); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -175,18 +224,22 @@ int StepperMotor::alignSensor() { } // check pole pair number 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! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { + SIMPLEFOC_DEBUG("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); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); @@ -200,7 +253,7 @@ int StepperMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -229,7 +282,7 @@ int StepperMotor::absoluteZeroSearch() { // check if the zero found if(monitor_port){ if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -245,8 +298,6 @@ void StepperMotor::loopFOC() { // 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; @@ -255,7 +306,40 @@ void StepperMotor::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 + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + 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.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -267,6 +351,9 @@ void StepperMotor::loopFOC() { // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target) ) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -285,63 +372,76 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; - // choose control loop + // upgrade the current based voltage limit switch (controller) { case MotionControlType::torque: - if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + 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 + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // 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 - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } 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 // if torque controlled through voltage control - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } 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; // TODO d-component lag-compensation + 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; // TODO d-component lag-compensation + voltage.d = 0; break; } } @@ -357,12 +457,9 @@ void StepperMotor::move(float new_target) { void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation + float _sa, _ca; + _sincos(angle_el, &_sa, &_ca); - // 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 Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; @@ -439,4 +536,4 @@ float StepperMotor::angleOpenloop(float target_angle){ open_loop_timestamp = now_us; return Uq; -} \ No newline at end of file +} diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 36f4fe3f..7e7810d8 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -43,7 +43,7 @@ class StepperMotor: public FOCMotor StepperDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ @@ -54,12 +54,8 @@ class StepperMotor: public FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -77,8 +73,6 @@ class StepperMotor: public FOCMotor */ void move(float target = NOT_SET) override; - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm @@ -95,6 +89,8 @@ class StepperMotor: public FOCMotor int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroSearch(); + /** Current sense and motor phase alignment */ + int alignCurrentSense(); // Open loop motion control /** diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index f405d785..6ae8186f 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -2,40 +2,17 @@ #define BLDCDRIVER_H #include "Arduino.h" +#include "FOCDriver.h" - -enum PhaseState : uint8_t { - PHASE_OFF = 0, // both sides of the phase are off - PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode - PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) - PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) -}; - - -class BLDCDriver{ +class BLDCDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - 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 - 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 + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -51,6 +28,9 @@ class BLDCDriver{ * @param sa - phase C state : active / disabled ( high impedance ) */ virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::BLDC; }; }; #endif diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index becfa499..4813dc3b 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -1,4 +1,5 @@ #include "CurrentSense.h" +#include "../../communication/SimpleFOCDebug.h" // get current magnitude @@ -7,63 +8,70 @@ 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 - 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; - }if(!current.a){ - // if only two measured currents - float a = -current.c - current.b; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ - // if only two measured currents - float b = -current.a - current.c; - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; - }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; - } + ABCurrent_s ABcurrent = getABCurrents(current); + + // current sign - if motor angle not provided the magnitude is always positive + float sign = 1; // 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; + if(motor_electrical_angle) { + float ct; + float st; + _sincos(motor_electrical_angle, &st, &ct); + sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1; + } // return current magnitude - return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); + return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents -// - using getPhaseCurrents internally +// - using getPhaseCurrents and getABCurrents internally DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); + // calculate clarke transform + ABCurrent_s ABcurrent = getABCurrents(current); + + // calculate park transform + DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el); + + return return_current; +} + +// function used with the foc algorithm +// calculating Alpha Beta currents from phase currents +// - function calculating Clarke transform of the phase currents +ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + + // check if driver is an instance of StepperDriver + // if so there is no need to Clarke transform + if (driver_type == DriverType::Stepper){ + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + return return_ABcurrent; + } + + // otherwise it's a BLDC motor and // 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; - }if(!current.a){ + }else if(!current.a){ // if only two measured currents float a = -current.c - current.b; i_alpha = a; i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; @@ -77,18 +85,391 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } - // calculate park transform - float ct = _cos(angle_el); - float st = _sin(angle_el); + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = i_alpha; + return_ABcurrent.beta = i_beta; + return return_ABcurrent; +} + +// function used with the foc algorithm +// calculating D and Q currents from Alpha Beta currents and electrical angle +// - function calculating Clarke transform of the phase currents +DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ + // calculate park transform + float ct; + float st; + _sincos(angle_el, &st, &ct); DQCurrent_s return_current; - return_current.d = i_alpha * ct + i_beta * st; - return_current.q = i_beta * ct - i_alpha * st; + return_current.d = current.alpha * ct + current.beta * st; + return_current.q = current.beta * ct - current.alpha * st; return return_current; } /** Driver linking to the current sense */ -void CurrentSense::linkDriver(BLDCDriver* _driver) { - driver = _driver; -} \ No newline at end of file +void CurrentSense::linkDriver(FOCDriver* _driver) { + driver = _driver; + // save the driver type for easier access + driver_type = driver->type(); +} + + +void CurrentSense::enable(){ + // nothing is done here, but you can override this function +}; + +void CurrentSense::disable(){ + // nothing is done here, but you can override this function +}; + + +// 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 +// IMPORTANT, this function can be overriden in the child class +int CurrentSense::driverAlign(float voltage, bool modulation_centered){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + if (!initialized) return 0; + + // check if stepper or BLDC + if(driver_type == DriverType::Stepper) + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); + else + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); +} + + + +// Helper function to read and average phase currents +PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { + PhaseCurrent_s c = getPhaseCurrents(); + for (int i = 0; i < N; 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); + } + return c; +}; + + + +// 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 CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ + + bool phases_switched = 0; + bool phases_inverted = 0; + + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; + + // set phase A active and phases B and C down + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero); + _delay(3); + } + _delay(500); + PhaseCurrent_s c_a = readAverageCurrents(); + bldc_driver->setPwm(zero, zero, zero); + // check if currents are to low (lower than 100mA) + // TODO calculate the 100mA threshold from the ADC resolution + // if yes throw an error and return 0 + // either the current sense is not connected or the current is + // too low for calibration purposes (one should raise the motor.voltage_sensor_align) + if((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I/2 on the phase B and I/2 on the phase C + + // find the highest magnitude in c_a + // and make sure it's around 2 (1.5 at least) times higher than the other two + float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!ca[i]) continue; // current not measured + if(ca[i] > max_c){ + max_c = ca[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!ca[j]) continue; // current not measured + float ratio = max_c / ca[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + + // check the current magnitude ratios + // 1) if there is one current that is approximately 2 times higher than the other two + // this is the A current + // 2) if the max current is not at least 1.5 times higher than the other two + // we have two cases: + // - either we only measure two currents and the third one is not measured - then phase A is not measured + // - or the current sense is not connected properly + + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + phases_switched = true; // signal that pins have been switched + break; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.a, c_a.c); + phases_switched = true;// signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_a.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!"); + return 0; + }else{ //phase A is not measured so put the _NC to the phase A + if(_isset(pinA) && !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch A-(B)NC"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + phases_switched = true; // signal that pins have been switched + }else if(_isset(pinA) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.b, c_a.c); + phases_switched = true; // signal that pins have been switched + } + } + // at this point the current sensing on phase A can be either: + // - aligned with the driver phase A + // - or the phase A is not measured and the _NC is connected to the phase A + // + // In either case A is done, now we have to check the phase B and C + + + // set phase B active and phases A and C down + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero); + _delay(3); + } + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(zero, zero, zero); + + // check the phase B + // find the highest magnitude in c_b + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)}; + max_i = -1; // max index + max_c = 0; // max current + max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cb[i]) continue; // current not measured + if(cb[i] > max_c){ + max_c = cb[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cb[j]) continue; // current not measured + float ratio = max_c / cb[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >= 1.5f){ + switch (max_i){ + case 0: // phase A is the max current + // this is an error as phase A is already aligned + SIMPLEFOC_DEBUG("CS: Err align B"); + return 0; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_b.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!"); + return 0; + }else{ //phase B is not measured so put the _NC to the phase B + if(_isset(pinB) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch B-(C)NC"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + } + } + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + // + // In either case A and B is done, now we have to check the phase C + // phase C is also aligned if it is measured (not _NC) + // we have to check if the current is negative and invert the gain if so + if(_isset(pinC)){ + if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity) + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } + } + + // construct the return flag + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + +// 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 CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ + + _UNUSED(modulation_centered); + + bool phases_switched = 0; + bool phases_inverted = 0; + + if(!_isset(pinA) || !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); + return 0; + } + + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // 1) only one phase can be measured so we first measure which ADC pin corresponds + // to the phase A by comparing the magnitude + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + phases_switched = true; // signal that pins have been switched + } + // 2) check if measured current a is positive and invert if not + if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(0, voltage/100.0*((float)i)); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + + // phase B should be aligned + // 1) we just need to verify that it has been measured + if (fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current on B!"); + return 0; // measurement current too low + } + // 2) check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad9f926d..d3f7f8ed 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,12 +1,15 @@ #ifndef CURRENTSENSE_H #define CURRENTSENSE_H -#include "BLDCDriver.h" +#include "FOCDriver.h" #include "../foc_utils.h" +#include "../time_utils.h" +#include "StepperDriver.h" +#include "BLDCDriver.h" /** * Current sensing abstract class defintion - * Each current sensoring implementation needs to extend this interface + * Each current sensing implementation needs to extend this interface */ class CurrentSense{ public: @@ -23,24 +26,49 @@ class CurrentSense{ * Linking the current sense with the motor driver * Only necessary if synchronisation in between the two is required */ - void linkDriver(BLDCDriver *driver); + void linkDriver(FOCDriver *driver); // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver = nullptr; //!< driver link + FOCDriver* driver = nullptr; //!< driver link bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing + DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper) + + // ADC measurement 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 + + 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) + + // 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 + /** * 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 + * @returns - + 0 - failure + 1 - success and nothing changed + 2 - success but pins reconfigured + 3 - success but gains inverted + 4 - success but pins reconfigured and gains inverted + * + * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage) = 0; + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -53,7 +81,7 @@ class CurrentSense{ 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 + * It returns the absolute 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) * @@ -62,13 +90,55 @@ class CurrentSense{ virtual float getDCCurrent(float angle_el = 0); /** - * Function used for FOC contorl, it reads the DQ currents of the motor + * Function used for FOC control, 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); + /** + * Function used for Clarke transform in FOC control + * It reads the phase currents of the motor + * It returns the alpha and beta currents + * + * @param current - phase current + */ + ABCurrent_s getABCurrents(PhaseCurrent_s current); + + /** + * Function used for Park transform in FOC control + * It reads the Alpha Beta currents and electrical angle of the motor + * It returns the D and Q currents + * + * @param current - phase current + */ + DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + + /** + * enable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void enable(); + + /** + * disable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void disable(); + + /** + * Function used to align the current sense with the BLDC motor driver + */ + int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); + /** + * Function used to align the current sense with the Stepper motor driver + */ + int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); + /** + * Function used to read the average current values over N samples + */ + PhaseCurrent_s readAverageCurrents(int N=100); }; diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h new file mode 100644 index 00000000..944251a4 --- /dev/null +++ b/src/common/base_classes/FOCDriver.h @@ -0,0 +1,47 @@ +#ifndef FOCDRIVER_H +#define FOCDRIVER_H + +#include "Arduino.h" + + +enum PhaseState : uint8_t { + PHASE_OFF = 0, // both sides of the phase are off + PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode + PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) + PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) +}; + + +enum DriverType{ + Unknown=0, + BLDC=1, + Stepper=2 +}; + +/** + * FOC driver class + */ +class FOCDriver{ + public: + + /** Initialise hardware */ + virtual int init() = 0; + /** Enable hardware */ + virtual void enable() = 0; + /** Disable hardware */ + virtual void disable() = 0; + + 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 + void* params = 0; //!< pointer to hardware specific parameters of driver + + bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) + + /** get the driver type*/ + virtual DriverType type() = 0; +}; + +#endif diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 7f48037f..5d8f8127 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -32,6 +32,10 @@ FOCMotor::FOCMotor() // voltage bemf voltage_bemf = 0; + + // Initialize phase voltages U alpha and U beta used for inverse Park and Clarke transform + Ualpha = 0; + Ubeta = 0; //monitor_port monitor_port = nullptr; @@ -82,14 +86,16 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print + #ifndef SIMPLEFOC_DISABLE_DEBUG SimpleFOCDebug::enable(&print); SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); + #endif } // 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; + if( !monitor_downsample || monitor_cnt++ < (monitor_downsample-1) ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c499df4a..8ae0e8af 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -78,7 +78,7 @@ class FOCMotor FOCMotor(); /** Motor hardware init function */ - virtual void init()=0; + virtual int init()=0; /** Motor disable function */ virtual void disable()=0; /** Motor enable function */ @@ -104,12 +104,8 @@ class FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + virtual int initFOC()=0; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -155,6 +151,7 @@ class FOCMotor // state variables float target; //!< current target value - depends of the controller + float feed_forward_velocity = 0.0f; //!< current feed forward velocity float shaft_angle;//!< current motor angle float electrical_angle;//!< current electrical angle float shaft_velocity;//!< current motor velocity @@ -164,6 +161,8 @@ class FOCMotor DQVoltage_s voltage;//!< current d and q voltage set to the motor DQCurrent_s current;//!< current d and q current measured float voltage_bemf; //!< estimated backemf voltage (if provided KV constant) + float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter @@ -208,7 +207,8 @@ 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 sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW + Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration + bool pp_check_result = false; //!< the result of the PP check, if run during loopFOC /** * Function providing BLDCMotor class with the diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 28511762..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -6,6 +6,8 @@ void Sensor::update() { float val = getSensorAngle(); + if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors. + return; // TODO signal error, e.g. via a flag and counter angle_prev_ts = _micros(); float d_angle = val - angle_prev; // if overflow happened track it as full rotation @@ -17,8 +19,13 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; - // TODO handle overflow - we do need to reset vel_angle_prev_ts + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; + if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; + return velocity; + } if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index b8834e8e..c77eb911 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -42,6 +42,7 @@ enum Pullup : uint8_t { * */ class Sensor{ + friend class SmoothingSensor; public: /** * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..9864b235 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,32 +1,30 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H -#include "drivers/hardware_api.h" +#include "Arduino.h" +#include "FOCDriver.h" -class StepperDriver{ +class StepperDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - 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 - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua phase A voltage * @param Ub phase B voltage */ virtual void setPwm(float Ua, float Ub) = 0; + + /** + * Set phase state, enable/disable + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::Stepper; } ; }; #endif \ No newline at end of file diff --git a/src/common/defaults.h b/src/common/defaults.h index c0a46182..ac57daa1 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,7 +43,7 @@ // align voltage #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 +#define DEF_VEL_FILTER_Tf 0.005f //!< 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 diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 11a1810f..7ae372f7 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,34 +1,31 @@ #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 -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) -// ~50us (int array) -// precision +-0.005 -// it has to receive an angle in between 0 and 2PI -float _sin(float a){ - if(a < _PI_2){ - //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.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.0f*((a - _PI) / (_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 { - // 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.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized +// uses a 65 element lookup table and interpolation +// thanks to @dekutree for his work on optimizing this +__attribute__((weak)) float _sin(float a){ + // 16bit integer array for sine lookup. interpolation is used for better precision + // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size + // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) + static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; + int32_t t1, t2; + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); + int frac = i & 0xff; + i = (i >> 8) & 0xff; + if (i < 64) { + t1 = (int32_t)sine_array[i]; t2 = (int32_t)sine_array[i+1]; + } + else if(i < 128) { + t1 = (int32_t)sine_array[128 - i]; t2 = (int32_t)sine_array[127 - i]; } + else if(i < 192) { + t1 = -(int32_t)sine_array[-128 + i]; t2 = -(int32_t)sine_array[-127 + i]; + } + else { + t1 = -(int32_t)sine_array[256 - i]; t2 = -(int32_t)sine_array[255 - i]; + } + return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } // function approximating cosine calculation by using fixed size array @@ -36,15 +33,48 @@ float _sin(float a){ // ~56us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -float _cos(float a){ +__attribute__((weak)) float _cos(float a){ float a_sin = a + _PI_2; a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; return _sin(a_sin); } +__attribute__((weak)) void _sincos(float a, float* s, float* c){ + *s = _sin(a); + *c = _cos(a); +} + +// fast_atan2 based on https://math.stackexchange.com/a/1105038/81278 +// Via Odrive project +// https://github.com/odriverobotics/ODrive/blob/master/Firmware/MotorControl/utils.cpp +// This function is MIT licenced, copyright Oskar Weigl/Odrive Robotics +// The origin for Odrive atan2 is public domain. Thanks to Odrive for making +// it easy to borrow. +__attribute__((weak)) float _atan2(float y, float x) { + // a := min (|x|, |y|) / max (|x|, |y|) + float abs_y = fabsf(y); + float abs_x = fabsf(x); + // inject FLT_MIN in denominator to avoid division by zero + float a = min(abs_x, abs_y) / (max(abs_x, abs_y)); + // s := a * a + float s = a * a; + // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a + float r = + ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; + // if |y| > |x| then r := 1.57079637 - r + if (abs_y > abs_x) r = 1.57079637f - r; + // if x < 0 then r := 3.14159274 - r + if (x < 0.0f) r = 3.14159274f - r; + // if y < 0 then r := -r + if (y < 0.0f) r = -r; + + return r; + } + + // normalizing radian angle to [0,2PI] -float _normalizeAngle(float angle){ +__attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } @@ -57,17 +87,11 @@ 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 _sqrtApprox(float number) {//low in fat - long i; - float y; - // float x; - // const float f = 1.5F; // better precision - - // x = number * 0.5F; - y = number; - i = * ( long * ) &y; - i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; - // y = y * ( f - ( x * y * y ) ); // better precision - return number * y; +__attribute__((weak)) float _sqrtApprox(float number) {//low in fat + union { + float f; + uint32_t i; + } y = { .f = number }; + y.i = 0x5f375a86 - ( y.i >> 1 ); + return number * y.f; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e1e0b153..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,6 +12,9 @@ #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) #define _UNUSED(v) (void) (v) +#define _powtwo(x) (1 << (x)) + +#define _swap(a, b) { auto temp = a; a = b; b = temp; } // utility defines #define _2_SQRT3 1.15470053838f @@ -28,11 +31,11 @@ #define _PI_6 0.52359877559f #define _RPM_TO_RADS 0.10471975512f -#define NOT_SET -12345.0 +#define NOT_SET -12345.0f #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -#define _NC (NOT_SET) +#define _NC ((int) NOT_SET) #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f) @@ -55,6 +58,12 @@ struct DQVoltage_s float d; float q; }; +// alpha beta current structure +struct ABCurrent_s +{ + float alpha; + float beta; +}; /** @@ -71,6 +80,18 @@ float _sin(float a); * @param a angle in between 0 and 2PI */ float _cos(float a); +/** + * Function returning both sine and cosine of the angle in one call. + * Internally it uses the _sin and _cos functions, but you may wish to + * provide your own implementation which is more optimized. + */ +void _sincos(float a, float* s, float* c); + +/** + * Function approximating atan2 + * + */ +float _atan2(float y, float x); /** * normalizing radian angle to [0,2PI] diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 7aa8865f..d2822b04 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -11,10 +11,10 @@ Commander::Commander(char eol, bool echo){ } -void Commander::add(char id, CommandCallback onCommand, char* label ){ +void Commander::add(char id, CommandCallback onCommand, const char* label ){ call_list[call_count] = onCommand; call_ids[call_count] = id; - call_label[call_count] = label; + call_label[call_count] = (char*)label; call_count++; } @@ -593,6 +593,7 @@ bool Commander::isSentinel(char ch) else if (ch == '\r') { printVerbose(F("Warn: \\r detected! \n")); + return true; // lets still consider it to end the line... } return false; } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 3633a510..4ec2b281 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -91,7 +91,7 @@ class Commander * @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); + void add(char id , CommandCallback onCommand, const char* label = nullptr); // printing variables VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text @@ -240,6 +240,7 @@ class Commander */ void motion(FOCMotor* motor, char* user_cmd, char* separator = (char *)" "); + bool isSentinel(char ch); private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number @@ -294,7 +295,6 @@ class Commander void printError(); - bool isSentinel(char ch); }; diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..4d50b87c 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -38,6 +38,7 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str) { } } + void SimpleFOCDebug::println(const char* str, float val) { if (_debugPrint != NULL) { _debugPrint->print(str); @@ -58,6 +59,12 @@ void SimpleFOCDebug::println(const char* str, int val) { _debugPrint->println(val); } } +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { if (_debugPrint != NULL) { @@ -80,6 +87,20 @@ void SimpleFOCDebug::print(const __FlashStringHelper* str) { } } +void SimpleFOCDebug::print(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->print(str.c_str()); + } +} + + +void SimpleFOCDebug::println(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->println(str.c_str()); + } +} + + void SimpleFOCDebug::print(int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..d0ff611f 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -32,25 +32,29 @@ * **/ +// #define SIMPLEFOC_DISABLE_DEBUG -#ifndef SIMPLEFOC_DISABLE_DEBUG +#ifndef SIMPLEFOC_DISABLE_DEBUG class SimpleFOCDebug { public: static void enable(Print* debugPrint = &Serial); static void println(const __FlashStringHelper* msg); + static void println(const StringSumHelper 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(const char* msg, char val); static void println(); static void println(int val); static void println(float val); static void print(const char* msg); static void print(const __FlashStringHelper* msg); + static void print(const StringSumHelper msg); static void print(int val); static void print(float val); @@ -62,10 +66,6 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) - - - - #else //ifndef SIMPLEFOC_DISABLE_DEBUG diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 9ae13362..3627b5e7 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,11 +5,6 @@ #include "../common/foc_utils.h" -#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE) || defined(CORE_TEENSY) || defined(NRF52_SERIES) -#define PinStatus int -#endif - - /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -53,7 +48,7 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing - PinStatus polarity = RISING; //!< polarity of the step pin + decltype(RISING) polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index d0592ef2..54c4f12e 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,108 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // 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(float voltage){ +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ _UNUSED(voltage) ; // remove unused parameter warning 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 + if (!initialized) return 0; return exit_flag; } diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index a63df49d..02bf8fa9 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; + int driverAlign(float align_voltage, bool modulation_centered) 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 6a41484a..35c97765 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -1,4 +1,5 @@ #include "InlineCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // InlineCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -43,8 +44,14 @@ int InlineCurrentSense::init(){ params = _configureADCInline(drv_params,pinA,pinB,pinC); // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -79,167 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// 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 InlineCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if(_isset(pinA)){ - // 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.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 = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - 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; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - 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; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s 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.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 = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - 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; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - 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 C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 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; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - 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/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 5fdca7d7..94be0f75 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -6,10 +6,13 @@ #include "../common/time_utils.h" #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" + class InlineCurrentSense: public CurrentSense{ public: /** @@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() 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 - // this should be automated later - float gain_a; //!< phase A gain - 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 - 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) - 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 float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 21df16e1..a0026ae3 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -1,4 +1,5 @@ #include "LowsideCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // LowsideCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -35,14 +36,27 @@ LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int // Lowside sensor init function int LowsideCurrentSense::init(){ + + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: Driver not linked!"); + return 0; + } + // configure ADC variables 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(driver->params, params); + void* r = _driverSyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -79,167 +93,3 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// 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(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if(_isset(pinA)){ - // 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.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 = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - 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; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - 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; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s 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.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 = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - 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; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - 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 C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 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; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - 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 index 1652b330..6651bcd2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -7,6 +7,8 @@ #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,30 +36,9 @@ class LowsideCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() 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 - // this should be automated later - float gain_a; //!< phase A gain - 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 - - 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) 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 float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7862b708..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -59,7 +59,10 @@ 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 + * + * @return void* - returns the pointer to the current sense parameter structure (unchanged) + * - returns SIMPLEFOC_CURRENT_SENSE_INIT_FAILED if the init fails */ -void _driverSyncLowSide(void* driver_params, void* cs_params); +void* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 807c387d..caf29c19 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,88 +1,22 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) - -#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); -} +#include "esp32_mcu.h" +#include "esp32_adc_driver.h" -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); -} +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 -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; - } +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits +#include "soc/sens_reg.h" +// configure the ADCs in RTC mode +// saves about 3us per call +// going from 12us to 9us +void __configFastADCs(){ + + // configure both ADCs in RTC mode 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); @@ -99,161 +33,144 @@ void IRAM_ATTR __analogInit(){ 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){ +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } + // start teh ADC conversion 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_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), 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 + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value 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 + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value 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); + // return value + return value; } -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} +#elif CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 // if esp32 s2 or s3 variants + +#include "soc/sens_reg.h" + + +// configure the ADCs in RTC mode +// no real gain - see if we do something with it later +// void __configFastADCs(){ + +// SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); +// SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); + +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + +// CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + +// CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); +// while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== +// } uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } - __analogInit(); - + // start the ADC conversion 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); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_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); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_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); + if(channel > 9){ + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_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); + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); + // read teh value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_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); + return value; } +#else // if others just use analogRead + +#pragma message("SimpleFOC: Using analogRead for ADC reading, no fast ADC configuration available!") + +uint16_t IRAM_ATTR adcRead(uint8_t pin){ + return analogRead(pin); +} + +#endif + + +// configure the ADC for the pin +bool IRAM_ATTR adcInit(uint8_t pin){ + static bool initialized = false; + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); + return false;//not adc pin + } + + if(! initialized){ + analogSetAttenuation(SIMPLEFOC_ADC_ATTEN); + analogReadResolution(SIMPLEFOC_ADC_RES); + } + pinMode(pin, ANALOG); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); + analogRead(pin); + +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant + __configFastADCs(); +#endif + + initialized = true; + return true; +} -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 869c4dde..cad441fc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,88 +1,24 @@ - - #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#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 + * @param pin - pin number + * @return ADC value (0 - 4095) * */ 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 +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin */ -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); +bool adcInit(uint8_t pin); -/* - * 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 /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ #endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp deleted file mode 100644 index f2d65f8e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#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 - -// 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(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/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..33d547db --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,158 @@ +#include "esp32_mcu.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +// check the version of the ESP-IDF +#include "esp_idf_version.h" + +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif + +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + + + +// adding a debug toggle pin to measure the time of the interrupt with oscilloscope + +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" + +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define DEBUGPIN 16 +#define GPIO_NUM GPIO_NUM_16 +#else +#define DEBUGPIN 19 +#define GPIO_NUM GPIO_NUM_19 +#endif + +#endif + + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32CurrentSenseParams* p = (ESP32CurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; + } + SIMPLEFOC_DEBUG("ERROR: ADC pin not found in the buffer!"); + // not found + return 0; +} + + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams{}; + int no_adc_channels = 0; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + int adc_pins[3] = {pinA, pinB, pinC}; + for (int i = 0; i < 3; i++){ + if(_isset(adc_pins[i])){ + if(!adcInit(adc_pins[i])){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + params->pins[no_adc_channels++] = adc_pins[i]; + } + } + + t->user_data = params; + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; + return params; +} + + + +void* _driverSyncLowSide(void* driver_params, void* cs_params){ +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(DEBUGPIN, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the callback for the low side current sensing + // mcpwm_timer_event_callbacks_t can be used to set the callback + // for three timer events + // - on_full - low-side + // - on_empty - high-side + // - on_sync - sync event (not used with simplefoc) + auto cbs = mcpwm_timer_event_callbacks_t{ + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32CurrentSenseParams *p = (ESP32CurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off +#endif + + // sample the phase currents one at a time + // ESP's adc read takes around 10us which is very long + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // so we are sampling one phase per call + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off +#endif + return true; + }, + }; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupt callback."); + // set the timer state to init (so that we can call the `mcpwm_timer_register_event_callbacks` ) + // this is a hack, as this function is not supposed to be called when the timer is running + // the timer does not really go to the init state! + t->fsm = MCPWM_TIMER_FSM_INIT; + // set the callback + CHECK_CS_ERR(mcpwm_timer_register_event_callbacks(t, &cbs, cs_params), "Failed to set low side callback"); + // set the timer state to enabled again + t->fsm = MCPWM_TIMER_FSM_ENABLE; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupts."); + CHECK_CS_ERR(esp_intr_enable(t->intr), "Failed to enable low-side interrupts!"); + + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index dd336416..e5ed3fbf 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -1,29 +1,6 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" - -#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" -#include "soc/mcpwm_struct.h" - -#include -#include - -#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; +#include "esp32_mcu.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Inline adc reading implementation @@ -31,132 +8,31 @@ typedef struct ESP32MCPWMCurrentSenseParams { // 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 * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; + return raw_adc * ((ESP32CurrentSenseParams*)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); - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; - return params; -} - - - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// 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){ - 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; - - 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; + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } } - // not found - return 0; -} - -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; return params; } -void _driverSyncLowSide(void* driver_params, void* cs_params){ - - 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 - 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 = 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){ - 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; - } - // 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 = 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){ - 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; - } - // 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; -} - - #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h new file mode 100644 index 00000000..9207fb6a --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -0,0 +1,37 @@ +#ifndef ESP32_MCU_CURRENT_SENSING_H +#define ESP32_MCU_CURRENT_SENSING_H + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + + +#include "../../../drivers/hardware_api.h" +#include "esp32_adc_driver.h" + + +// esp32 current sense parameters +typedef struct ESP32CurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32CurrentSenseParams; + +// macros for debugging wuing the simplefoc debug system +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-CS: "+ String(str));\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +#endif // ESP_H && ARDUINO_ARCH_ESP32 +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp deleted file mode 100644 index 2e956770..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ /dev/null @@ -1,257 +0,0 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) - -#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_READER1_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_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_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_CONF_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_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_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_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_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_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_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_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) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - __analogInit(); - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - - uint16_t value = 0; - - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_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/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../communication/SimpleFOCDebug.h" // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ @@ -24,18 +25,23 @@ __attribute__((weak)) void* _configureADCInline(const void* driver_params, cons // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ - return _readADCVoltageInline(pinA, cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return 0.0; } // Configure low side for generic mcu // cannot do much but __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - return _configureADCInline(driver_params, pinA, pinB, pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ _UNUSED(driver_params); - _UNUSED(cs_params); + return cs_params; } + +// function starting the ADC conversion for the high side current sensing +// only necessary for certain types of MCUs __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 7bcd23aa..d2ed881b 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -10,6 +10,7 @@ #include "hardware/dma.h" #include "hardware/irq.h" #include "hardware/pwm.h" +#include "hardware/adc.h" /* Singleton instance of the ADC engine */ @@ -24,6 +25,7 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-( // like this we either have to block interrupts, or of course have the chance of reading across // new ADC conversions, which probably won't improve the accuracy. + _UNUSED(cs_params); if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) { return engine.lastResults.raw[pinA-26]*engine.adc_conv; @@ -35,6 +37,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) { + _UNUSED(driver_params); + if( _isset(pinA) ) engine.addPin(pinA); if( _isset(pinB) ) @@ -82,7 +86,7 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p // }; -// void _driverSyncLowSide(void* driver_params, void* cs_params) { +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { // // nothing to do // }; @@ -163,7 +167,6 @@ bool RP2040ADCEngine::init() { false, // We won't see the ERR bit because of 8 bit reads; disable. true // Shift each sample to 8 bits when pushing to FIFO ); - samples_per_second = 20000; if (samples_per_second<1 || samples_per_second>=500000) { samples_per_second = 0; adc_set_clkdiv(0); @@ -241,6 +244,7 @@ void RP2040ADCEngine::start() { void RP2040ADCEngine::stop() { adc_run(false); + irq_set_enabled(DMA_IRQ_0, false); dma_channel_abort(readDMAChannel); // if (triggerPWMSlice>=0) // dma_channel_abort(triggerDMAChannel); diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h index b673fc30..ae28bb26 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h @@ -22,6 +22,9 @@ * Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz. * After starting the engine it will continuously sample and provide new values at the configured rate. * + * The default sampling rate is 20kHz, which is suitable for 2 channels assuming you a 5kHz main loop speed (a new measurement is used per + * main loop iteration). + * * Low-side sensing is currently not supported. * * The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current @@ -74,7 +77,7 @@ class RP2040ADCEngine { ADCResults getLastResults(); // TODO find a better API and representation for this - int samples_per_second = 0; // leave at 0 to convert in tight loop + int samples_per_second = 20000; // 20kHz default (assuming 2 shunts and 5kHz loop speed), set to 0 to convert in tight loop float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float //int triggerPWMSlice = -1; diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index da5ba85b..046f3db4 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -54,14 +54,15 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide(void* driver_params, void* cs_params) +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 + + return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 3e10bbca..46cb20be 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -27,7 +27,7 @@ volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the resul // 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, const void* cs_params){ +float _readADCVoltageLowSide(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]; @@ -110,6 +110,9 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p MX_ADC1_Init(&hadc1); MX_ADC2_Init(&hadc2); + HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); + MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); @@ -145,7 +148,7 @@ void DMA1_Channel2_IRQHandler(void) { } } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -166,6 +169,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..d3bea81e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -67,7 +67,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.DiscontinuousConvMode = DISABLE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 0; + hadc.Init.NbrOfConversion = 1; HAL_ADC_Init(&hadc); /**Configure for the selected ADC regular channel to be converted. */ @@ -124,10 +124,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - cs_params->adc_handle = &hadc; return 0; @@ -156,7 +152,6 @@ extern "C" { { HAL_ADC_IRQHandler(&hadc); } - } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 8daa7eef..49f2f3d5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -19,6 +19,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ if(AdcHandle->Instance == ADC1) return 0; #ifdef ADC2 // if ADC2 exists @@ -42,12 +48,12 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -63,27 +69,58 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle); + // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + if(use_adc_interrupt){ + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index 68a9d094..bd0df4b6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,10 +135,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - // enable interrupt - HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC_IRQn); - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 60344781..6b597d4e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -22,6 +22,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -34,12 +40,12 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -55,27 +61,55 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // start the adc + if (use_adc_interrupt){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp new file mode 100644 index 00000000..d4cffec6 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -0,0 +1,185 @@ +#include "stm32f7_hal.h" + +#if defined(STM32F7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +ADC_HandleTypeDef hadc; + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + // check if all pins belong to the same ADC + ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; + if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); +#endif + return -1; + } + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef ADC3 // if defined ADC3 + else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE(); +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 1; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; + + // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; + // if(TRGO_already_configured) continue; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = timer_to_check; + if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) { + // workaround for errata 2.2.1 in ES0290 Rev 7 + // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf + __HAL_RCC_DAC_CLK_ENABLE(); + } + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); + #endif + return -1; + } + // display which timer is being used + #ifdef SIMPLEFOC_STM32_DEBUG + // it would be better to use the getTimerNumber from driver + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + #endif + + + // first channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); +#endif + return -1; + } + + // second channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; +#endif + return -1; + } + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif + return -1; + } + } + + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif + + cs_params->adc_handle = &hadc; + return 0; +} + +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + uint8_t cnt = 0; + if(_isset(pinA)){ + pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); + cs_params->pins[cnt++] = pinA; + } + if(_isset(pinB)){ + pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); + cs_params->pins[cnt++] = pinB; + } + if(_isset(pinC)){ + pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); + cs_params->pins[cnt] = pinC; + } +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h new file mode 100644 index 00000000..0a3614b5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -0,0 +1,15 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) +#include "stm32f7xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32f7_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp new file mode 100644 index 00000000..f5ca70f3 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -0,0 +1,116 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F7xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32f7_hal.h" +#include "stm32f7_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (3) +uint32_t adc_val[3][4]={0}; +// does adc interrupt need a downsample - per adc (3) +bool needs_downsample[3] = {1}; +// downsampling variable - per adc (3) +uint8_t tim_downsample[3] = {1}; + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + // stop all the timers for the driver + _stopTimers(driver_params->timers, 6); + + // if timer has repetition counter - it will downsample using it + // and it does not need the software downsample + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + // adjust the initial timer state such that the trigger + // - for DMA transfer aligns with the pwm peaks instead of throughs. + // - for interrupt based ADC transfer + // - only necessary for the timers that have repetition counters + + cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + // remember that this timer has repetition counter - no need to downasmple + needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + } + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp new file mode 100644 index 00000000..d5f8c6b2 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -0,0 +1,255 @@ +#include "stm32f7_utils.h" + +#if defined(STM32F7xx) + +/* Exported Functions */ + + +PinName analog_to_pin(uint32_t pin) { + PinName pin_name = analogInputToPinName(pin); + if (pin_name == NC) { + return (PinName) pin; + } + return pin_name; +} + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} +/* +TIM1 +TIM2 +TIM3 +TIM4 +TIM5 +TIM6 +TIM7 +TIM12 +TIM13 +TIM14 + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO +ADC_EXTERNALTRIGINJECCONV_T2_TRGO +ADC_EXTERNALTRIGINJECCONV_T4_TRGO + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T8_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T5_TRGO +ADC_EXTERNALTRIGINJECCONV_T6_TRGO +*/ +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} +/* + +ADC_EXTERNALTRIGCONV_T5_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO2 +ADC_EXTERNALTRIGCONV_T1_TRGO +ADC_EXTERNALTRIGCONV_T1_TRGO2 +ADC_EXTERNALTRIGCONV_T2_TRGO +ADC_EXTERNALTRIGCONV_T4_TRGO +ADC_EXTERNALTRIGCONV_T6_TRGO +*/ + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h new file mode 100644 index 00000000..017ff464 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -0,0 +1,30 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin); + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 91322112..fd1090ae 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -100,7 +100,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; @@ -180,42 +180,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - - - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index ae96bad7..9c73f6d7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -11,6 +11,7 @@ #include "stm32g4_utils.h" #include "Arduino.h" +// #define SIMPLEFOC_STM32_ADC_INTERRUPT #define _ADC_VOLTAGE_G4 3.3f #define _ADC_RESOLUTION_G4 4096.0f @@ -23,6 +24,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -36,12 +42,12 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -57,27 +63,90 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + + // start the adc + if (use_adc_interrupt){ + // enable interrupt + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..67a0473b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -99,7 +99,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 2; + hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; @@ -179,42 +179,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - - - if(hadc.Instance == ADC1) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#ifdef ADC2 - else if (hadc.Instance == ADC2) { - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - } -#endif -#ifdef ADC3 - else if (hadc.Instance == ADC3) { - // enable interrupt - HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC3_IRQn); - } -#endif -#ifdef ADC4 - else if (hadc.Instance == ADC4) { - // enable interrupt - HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC4_IRQn); - } -#endif -#ifdef ADC5 - else if (hadc.Instance == ADC5) { - // enable interrupt - HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC5_IRQn); - } -#endif - cs_params->adc_handle = &hadc; return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index edac6414..5de6432a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -23,6 +23,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -36,12 +41,12 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -57,27 +62,87 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + + // start the adc + if (use_adc_interrupt){ + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + if (use_adc_interrupt){ + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + }else{ + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + } } return 0; } - extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..3a542b53 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,249 @@ +#include "teensy4_mcu.h" +#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +// #include "../../../common/lowpass_filter.h" +#include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +// #define SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + + +volatile uint32_t val0, val1, val2; + +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense +// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); + +void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ + *a = val0; + *b = val1; + *c = val2; +} + +// interrupt service routine for the ADC_ETC0 +// reading the ADC values and clearing the interrupt +void adcetc0_isr() { +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit + // val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095); + val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); + // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); + val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif +} + + +void adcetc1_isr() { +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit + val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; +// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) (1 is faster and maybe with some filtering could provide better results but 2 for now) + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + + //Calibration of ADC1 + ADC1_GC |= ADC_GC_CAL; // begin cal ADC1 + while (ADC1_GC & ADC_GC_CAL) ; + + ADC1_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + if(_isset(pin3)) { + ADC1_HC1 = 16; + } +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc2_init(){ + + // configuring ADC2 + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + //Calibration of ADC2 + ADC2_GC |= ADC_GC_CAL; // begin cal ADC2 + while (ADC2_GC & ADC_GC_CAL) ; + + ADC2_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC2_HC1 = 16; +} + +// function initializing the ADC_ETC trigger for the low side current sensing +// it uses only the ADC1 +// if the pin3 is not set it uses only 2 channels +void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { + ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST + ADC_ETC_CTRL = 0x40000001; // start with trigger 0 + ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels + + // ADC1 7 8, chain channel, HWTS, IE, B2B + // pg 3516, section 66.5.1.8 + ADC_ETC_TRIG0_CHAIN_1_0 = + ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS1(1) | + ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8 + ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7 + + attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); + // use the second interrupt if necessary (for more than 2 channels) + if(_isset(pin3)) { + ADC_ETC_TRIG0_CHAIN_3_2 = + ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]); + + attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + } +} + + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + + if(!_isset(pinA)) return 0.0; // if the pin is not set return 0 + GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; + float adc_voltage_conv = params->adc_voltage_conv; + if (pinA == params->pins[0]) { + return val0 * adc_voltage_conv; + } else if (pinA == params->pins[1]) { + return val1 * adc_voltage_conv; + }else if (pinA == params->pins[2]) { + return val2 * adc_voltage_conv; + } + return 0.0; +} + +// Configure low side for generic mcu +// cannot do much but +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + if(par == nullptr){ + SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); + +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + pinMode(30,OUTPUT); +#endif + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // check if either of the pins are not set + // and dont use it if it isn't + int pin_count = 0; + int pins[3] = {NOT_SET, NOT_SET, NOT_SET}; + if(_isset(pinA)) pins[pin_count++] = pinA; + if(_isset(pinB)) pins[pin_count++] = pinB; + if(_isset(pinC)) pins[pin_count++] = pinC; + + + adc1_init(pins[0], pins[1], pins[2]); + adc_etc_init(pins[0], pins[1], pins[2]); + + xbar_init(); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = {pins[0], pins[1], pins[2] }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + return params; +} + +// sync driver and the adc +void* _driverSyncLowSide(void* driver_params, void* cs_params){ + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; + int submodule = par->submodules[0]; + + SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!"); + char buff[50]; + sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule); + SIMPLEFOC_DEBUG(buff); + + // find the xbar trigger for the flexpwm + int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); + if(xbar_trig_pwm<0) return; + + // allow theFlexPWM to trigger the ADC_ETC + xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1) + //This val1 interrupt on match is in the center of the PWM + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1); + + + // if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4 + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) + // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // setup this val4 for interrupt on match for ADC sync + // this code assumes that the val4 is not used for anything else! + // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center + // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center + + +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG + // pin 4 observes out trigger line for 'scope + xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; + IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 + // Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO) + CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ; + // turn up drive & speed as very short pulse + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; +#endif + + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.h b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h new file mode 100644 index 00000000..2cf77dfb --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,77 @@ + +#ifndef TEENSY4_CURRENTSENSE_MCU_DEF +#define TEENSY4_CURRENTSENSE_MCU_DEF + +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4026.0f + +// 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 Teensy4CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; +} Teensy4CurrentSenseParams; + + + +const uint8_t pin_to_channel[] = { // pg 482 + 7, // 0/A0 AD_B1_02 + 8, // 1/A1 AD_B1_03 + 12, // 2/A2 AD_B1_07 + 11, // 3/A3 AD_B1_06 + 6, // 4/A4 AD_B1_01 + 5, // 5/A5 AD_B1_00 + 15, // 6/A6 AD_B1_10 + 0, // 7/A7 AD_B1_11 + 13, // 8/A8 AD_B1_08 + 14, // 9/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 + 7, // 14/A0 AD_B1_02 + 8, // 15/A1 AD_B1_03 + 12, // 16/A2 AD_B1_07 + 11, // 17/A3 AD_B1_06 + 6, // 18/A4 AD_B1_01 + 5, // 19/A5 AD_B1_00 + 15, // 20/A6 AD_B1_10 + 0, // 21/A7 AD_B1_11 + 13, // 22/A8 AD_B1_08 + 14, // 23/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 +#ifdef ARDUINO_TEENSY41 + 255, // 28 + 255, // 29 + 255, // 30 + 255, // 31 + 255, // 32 + 255, // 33 + 255, // 34 + 255, // 35 + 255, // 36 + 255, // 37 + 128+1, // 38/A14 AD_B1_12 - only on ADC2, 1 + 128+2, // 39/A15 AD_B1_13 - only on ADC2, 2 + 9, // 40/A16 AD_B1_04 + 10, // 41/A17 AD_B1_05 +#endif +}; + + +#endif + +#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/teensy_mcu.cpp similarity index 94% rename from src/current_sense/hardware_specific/teensy_mcu.cpp rename to src/current_sense/hardware_specific/teensy/teensy_mcu.cpp index 7ab370a4..7669edc8 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index 1942f60b..75ee478c 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,10 +38,9 @@ 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 + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -50,7 +49,8 @@ class BLDCDriver3PWM: public BLDCDriver void setPwm(float Ua, float Ub, float Uc) override; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for all phases! * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index e8643cc5..7ba7631c 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,7 +37,6 @@ 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] diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e8ccc6c6 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -44,8 +44,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 ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -56,8 +56,8 @@ void StepperDriver2PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -84,6 +84,14 @@ int StepperDriver2PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} // Set voltage to the pwm pin void StepperDriver2PWM::setPwm(float Ua, float Ub) { diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index b349af06..1bd00db9 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -60,6 +60,15 @@ class StepperDriver2PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + /** + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..52f1c1d1 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -21,8 +21,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 ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -33,8 +33,8 @@ void StepperDriver4PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -59,6 +59,15 @@ int StepperDriver4PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} + // Set voltage to the pwm pin void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { diff --git a/src/drivers/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h index e4b2ee42..33e7d0cf 100644 --- a/src/drivers/StepperDriver4PWM.h +++ b/src/drivers/StepperDriver4PWM.h @@ -47,6 +47,16 @@ class StepperDriver4PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + + /** + * Set phase voltages to the hardware. + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -25,9 +25,6 @@ #define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif - - - // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 5ad3653c..532b3ce3 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__AVR_ATmega2560__) || defined(AVR_ATmega1280) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega2560 or Arduino/ATmega1280") +#pragma message("") + + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 @@ -51,7 +57,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -69,7 +76,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -88,7 +96,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; // _syncAllTimers(); return params; @@ -135,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 385f6e64..9df9b8a7 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -2,6 +2,10 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega328 ATmega168 ATmega328PB") +#pragma message("") + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 @@ -49,7 +53,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -66,7 +71,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -85,7 +91,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; _syncAllTimers(); return params; @@ -137,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b9836e16..4cf454ae 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -3,6 +3,10 @@ #if defined(__AVR_ATmega32U4__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega32U4") +#pragma message("") + // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ // High PWM frequency @@ -37,7 +41,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -52,7 +57,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -68,7 +74,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -115,7 +122,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 80ef82d9..4397114b 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__arm__) && defined(__SAM3X8E__) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Due") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp new file mode 100644 index 00000000..a481c6ff --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -0,0 +1,508 @@ + +/* +* MCPWM in espressif v5.x has +* - 2x groups (units) +* each one has +* - 3 timers +* - 3 operators (that can be associated with any timer) +* which control a 2xPWM signals +* - 1x comparator + 1x generator per PWM signal + + +* Independent mode: +* ------------------ +* 6 PWM independent signals per unit +* unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* -------------------------------------------------------------------------------- +* 0-1 | 0-2 | 0 | 0 | 0 | A +* 0-1 | 0-2 | 0 | 1 | 1 | B +* 0-1 | 0-2 | 1 | 0 | 0 | A +* 0-1 | 0-2 | 1 | 1 | 1 | B +* 0-1 | 0-2 | 2 | 0 | 0 | A +* 0-1 | 0-2 | 2 | 1 | 1 | B +* +* ------------------------------------- Example 3PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 └─ operator 1 - comparator 0 - generator 0 - pwm C +* +* ------------------------------------- Example 2PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* unit - timer 0-2 - operator 0 -| +* 0-1 └─ comparator 1 - generator 1 -> pmw B +* +* -------------------------------------- Example 4PWM ----------------------------- +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 | ┌─ comparator 0 - generator 0 -> pwm C +* └─ operator 1 -| +* └─ comparator 0 - generator 0 -> pwm D + + +* Complementary mode +* ------------------ +* - : 3 pairs of complementary PWM signals per unit +* unit(0/1) > timer(0) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(high/low pair) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* ------------------------------------------------------------------------ +* 0-1 | 0 | 0 | 0 | 0 | A +* 0-1 | 0 | 0 | 1 | 1 | B +* 0-1 | 0 | 1 | 0 | 0 | A +* 0-1 | 0 | 1 | 1 | 1 | B +* 0-1 | 0 | 2 | 0 | 0 | A +* 0-1 | 0 | 2 | 1 | 1 | B +* +* -------------------------------------- Example 6PWM ----------------------------- +* +* ┌─ comparator 0 - generator 0 -> pwm A_h +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw A_l +* | +* unit | ┌─ comparator 0 - generator 0 -> pwm B_h +* (group) - timer 0 -|- operator 1 -| +* 0-1 | └─ comparator 1 - generator 1 -> pmw B_l +* | +* | ┌─ comparator 0 - generator 0 -> pwm C_h +* └─ operator 2 -| +* └─ comparator 1 - generator 1 -> pmw C_l +* + + +* More info +* ---------- +* - timers can be associated with any operator, and multiple operators can be associated with the same timer +* - comparators can be associated with any operator +* - two comparators per operator for independent mode +* - one comparator per operator for complementary mode +* - generators can be associated with any comparator +* - one generator per PWM signal for independent mode +* - two generators per pair of PWM signals for complementary mode (not used in simplefoc) +* - dead-time can be set for each generator pair in complementary mode +* +* Docs +* ------- +* More info here: https:*www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm +* and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +*/ + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "esp32_driver_mcpwm.h" + +// MCPWM driver hardware timer pointers +mcpwm_timer_handle_t timers[2][3] = {NULL}; +// MCPWM timer periods configured (directly related to the pwm frequency) +uint32_t pwm_periods[2][3]; +// how many pins from the groups 6 pins is used +uint8_t group_pins_used[2] = {0}; +// last operator in the group +mcpwm_oper_handle_t last_operator[2]; + + + +// checking if group has pins available +bool _hasAvailablePins(int group, int no_pins){ + if(group_pins_used[group] + no_pins > 6){ + return false; + } + return true; +} + +// returns the index of the last timer in the group +// -1 if no timer instantiated yet +uint8_t _findLastTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i-1; + } + } + // return the last index + return i; +} +// returns the index of the next timer to instantiate +// -1 if no timers available +uint8_t _findNextTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i; + } + } + return -1; +} + +/* + * find the best group for the pins + * if 6pwm + * - Only option is an an empty group + * if 3pwm + * - Best is an empty group (we can set a pwm frequency) + * - Second best is a group with 4pwms (2 operators) available (we can set the pwm frequency -> new timer+new operator) + * - Third best option is any group which has 3pwms available (but uses previously defined pwm_frequency) + * if 1pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms (one operator) available (we can set the pwm frequency -> new timer+new operator) + * - Third best is a group with 1pwm available (but uses previously defined pwm_frequency ) + * if 2pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms available (we can set the pwm frequency -> new timer+new operator) + * - Third best is one pin per group (but uses previously defined pwm_frequency ) + * if 4pwm + * - best option is an empty group (we can set the pwm frequency) + * - second best is a group with 4pwms available (we can set the pwm frequency -> new timer + new operators) + * - third best is 2pwms per group (we can set the pwm frequency -> new timers + new operators) + * + * PROBLEM: Skipping/loosing channels happens in some cases when the group has already used some odd number of pwm channels (for example 3pwm or 1pwm) + * For example if the group has already used 3pwms, there is one generator that has one pwm channel left. + * If we use this channel we have to use the same timer it has been used with before, so we cannot change the pwm frequency. + * Current implementation does use the remaining channel only if there isn't other options that would allow changing the pwm frequency. + * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel + * and use a new timer and operator to allow changing the pwm frequency. In such cases we loose (cannot be used) the remaining channel. + * TODO: use the pwm_frequency to avoid skipping pwm channels ! + * + * returns + * - 1 if solution found in one group + * - 2 if solution requires using both groups + * - 0 if no solution possible +*/ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ + // an empty group is always the best option + for(int i=0; i<2; i++){ + if(!group_pins_used[i]){ + *group = i; + *timer=0; // use the first timer in an empty group + return 1; + } + } + + // if 3 or 1pwm + // check if there is available space in one of the groups + // otherwise fail + if(no_pins == 3 || no_pins==1){ + // second best option is if there is a group with + // pair number of pwms available as we can then + // set the pwm frequency + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins+1)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findLastTimer(i); + return 1; + } + } + } + + // if 2 or 4 pwm + // check if there is available space in one of the groups + // if not check if they can be separated in two groups + if(no_pins == 2 || no_pins==4){ + // second best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is half pwms per group + int half_no_pins = (int)no_pins/2; + if(_hasAvailablePins(0,half_no_pins) && _hasAvailablePins(1 ,half_no_pins)){ + return 2; + } + } + + // otherwise fail + return 0; +} + + +// configuring center aligned pwm +// More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ + if(inverted) + return mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); + else + return mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); +} + + + +// Helper function calculating the pwm period from the pwm frequency +// - pwm_frequency - pwm frequency in hertz +// returns pwm period in ticks (uint32_t) +uint32_t _calcPWMPeriod(long pwm_frequency) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_frequency); +} +/* + Helper function calculating the pwm frequency from the pwm period + - pwm_period - pwm period in ticks + returns pwm frequency in hertz (long) +*/ +long _calcPWMFreq(long pwm_period) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_period / 2); +} + +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins){ + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + + uint8_t no_operators = 3; // use 3 comparators one per pair of pwms + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + +#else // software dead-time (software 6pwm) +// software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); + + int no_pins = 6; + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } +#endif + + int no_generators = 6; // one per pwm + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); + // Create and configure generators + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_generators; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]),"Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); + + } + // only available for 6pwm + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); + uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; + mcpwm_dead_time_config_t dt_config_high; + dt_config_high.posedge_delay_ticks = dead_time; + dt_config_high.negedge_delay_ticks = 0; + dt_config_high.flags.invert_output = !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH; + mcpwm_dead_time_config_t dt_config_low; + dt_config_low.posedge_delay_ticks = 0; + dt_config_low.negedge_delay_ticks = dead_time; + dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i+1], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + } +#else // software dead-time (software 6pwm) + for (int i = 0; i < 3; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); + } +#endif + + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); + params->dead_zone = dead_zone; + // save the configuration variables for later + group_pins_used[mcpwm_group] = 6; + return params; +} + + +/* + function configuring the pins for the mcpwm + - pwm_frequency - pwm frequency + - mcpwm_group - mcpwm group + - timer_no - timer number + - no_pins - number of pins + - pins - array of pins + - dead_zone - dead zone + + returns the driver parameters +*/ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins){ + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + bool shared_timer = false; + // check if timer is configured + if (timers[mcpwm_group][timer_no] == NULL){ + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + // initialise the timer + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + // save variables for later + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + // if the numer of used channels it not pair skip one channel + // the skipped channel cannot be used with the new timer + // TODO avoid loosing channels like this + if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; + }else{ + // we will use an already instantiated timer + params->timers[0] = timers[mcpwm_group][timer_no]; + SIMPLEFOC_ESP32_DRV_DEBUG("Using previously configured timer: " + String(timer_no)); + // but we cannot change its configuration without affecting the other drivers + // so let's first verify that the configuration is the same + if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ + SIMPLEFOC_ESP32_DRV_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); + + shared_timer = true; + } + + uint8_t no_operators = ceil(no_pins / 2.0); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + if (shared_timer && i == 0) { // first operator already configured + params->oper[0] = last_operator[mcpwm_group]; + continue; + } + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + // save the last operator in this group + last_operator[mcpwm_group] = params->oper[no_operators - 1]; + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + for (int i = 0; i < no_pins; i++) { + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); + } + + + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < no_pins; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH), "Failed to configure center align pwm: " + String(i)); + } + + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); + // save the configuration variables for later + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + group_pins_used[mcpwm_group] += no_pins; + return params; +} + +// function setting the duty cycle to the MCPWM pin +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = _constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +// function setting the duty cycle to the MCPWM pin +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ + // phase state is forced in hardware pwm mode + // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // github issue: https://github.com/espressif/esp-idf/issues/12237 + mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); + mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4f8ad131..5726e906 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,84 +5,154 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -#include "driver/mcpwm.h" +#include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" +#include "esp_idf_version.h" + +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif + +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif + +//!< ESP32 MCPWM driver parameters +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; //!< frequency of the pwm signal + int group_id; //!< group of the mcpwm + mcpwm_timer_handle_t timers[2]; //!< timers of the mcpwm + mcpwm_oper_handle_t oper[3]; //!< operators of the mcpwm + mcpwm_cmpr_handle_t comparator[6]; //!< comparators of the mcpwm + mcpwm_gen_handle_t generator[6]; //!< generators of the mcpwm + uint32_t mcpwm_period; //!< period of the pwm signal + float dead_zone; //!< dead zone of the pwm signal +} ESP32MCPWMDriverParams; + + +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); + +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ + +// macro for checking the error of the mcpwm functions +// if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED +#define CHECK_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_DRV_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_DRIVER_INIT_FAILED; \ + } -// 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 +#define _PWM_TIMEBASE_RESOLUTION_HZ (_MCPWM_FREQ) /*!< Resolution of MCPWM */ +// pwm frequency settings +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50kHz -// preferred pwm resolution default -#define _PWM_RES_DEF 4096 -// min resolution -#define _PWM_RES_MIN 3000 -// max resolution -#define _PWM_RES_MAX 8000 -// 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; +// low-level configuration API -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; +/** + * checking if group has pins available + * @param group - group of the mcpwm + * @param no_pins - number of pins + * @returns true if pins are available, false otherwise + */ +bool _hasAvailablePins(int group, int no_pins); +/** + * function finding the last timer in the group + * @param group - group of the mcpwm + * @returns index of the last timer in the group + * -1 if no timer instantiated yet + */ +uint8_t _findLastTimer(int group); + +/** + * function finding the next timer in the group + * @param group - group of the mcpwm + * @returns index of the next timer in the group + * -1 if all timers are used + */ +uint8_t _findNextTimer(int group); + + +/** + * function finding the best group and timer for the pwm signals + * + * @param no_pins - number of pins + * @param pwm_freq - frequency of the pwm signal + * @param group - pointer to the group + * @param timer - pointer to the timer + * @returns + * 1 if solution found in one group + * 2 if solution requires using both groups + * 0 if no solution possible + */ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); + + +/** + * function configuring the center alignement and inversion of a pwm signal + * @param gena - mcpwm generator handle + * @param cmpa - mcpwm comparator handle + * @param inverted - true if the signal is inverted, false otherwise + */ +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); + +/** + * function calculating the pwm period + * @param pwm_frequency - frequency of the pwm signal + * @return uint32_t - period of the pwm signal + */ +uint32_t _calcPWMPeriod(long pwm_frequency); +/** + * function calculating the pwm frequency + * @param pwm_period - period of the pwm signal + * @return long - frequency of the pwm signal + */ +long _calcPWMFreq(long pwm_period); + +/** + * function configuring the MCPWM for 6pwm + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param dead_zone - dead zone of the pwm signal + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins); +/** + * function configuring the MCPWM for pwm generation + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param no_pins - number of pins + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins); +/** + * function setting the duty cycle to the MCPWM channel + * @param cmpr - mcpwm channel handle + * @param mcpwm_period - period of the pwm signal + * @param duty_cycle - duty cycle of the pwm signal + */ +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * function setting the phase state + * @param generator_high - mcpwm generator handle for the high side + * @param generator_low - mcpwm generator handle for the low side + * @param phase_state - phase state + */ +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state); #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 76027459..c5ba1c78 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -2,19 +2,26 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 LEDC driver") +#pragma message("") + #include "driver/ledc.h" +#include "esp_idf_version.h" + + +// version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #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 +#define _PWM_RES 1023 // 2^10-1 = 1024-1 -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// figure out how many ledc channels are avaible +// figure out how many ledc channels are available // esp32 - 2x8=16 // esp32s2 - 8 // esp32c3 - 6 @@ -25,51 +32,130 @@ #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) + -// current channel stack index +// currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channel_index = 0; - - +// channels from 0-7 are in group 0 and 8-15 in group 1 +// - only esp32 as of mid 2024 has the second group, all the s versions don't +int group_channels_used[2] = {0}; typedef struct ESP32LEDCDriverParams { - int channels[6]; + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; long pwm_frequency; + float dead_zone; } ESP32LEDCDriverParams; - - - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); +/* + Function to attach a channel to a pin with advanced settings + - freq - pwm frequency + - resolution - pwm resolution + - channel - ledc channel + - inverted - output inverted + - group - ledc group + + This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the + PWM signals are synchronized in pairs, while the simplefoc requires a bit more flexible configuration. + This function sets also allows configuring a channel as inverted, which is not possible with the ledcAttachPin function. + + Function returns true if the channel was successfully attached, false otherwise. +*/ +bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t freq, uint8_t resolution, bool inverted) { + + + ledc_channel_t channel = static_cast(_channel); + ledc_mode_t group = static_cast(_group); + + ledc_timer_bit_t res = static_cast(resolution); + ledc_timer_config_t ledc_timer; + memset(&ledc_timer, 0, sizeof(ledc_timer)); + ledc_timer.speed_mode = group; + ledc_timer.timer_num = LEDC_TIMER_0; + ledc_timer.duty_resolution = res; + ledc_timer.freq_hz = freq; + ledc_timer.clk_cfg = LEDC_AUTO_CLK; + if (ledc_timer_config(&ledc_timer) != ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure the timer:", LEDC_TIMER_0); + return false; + } + + // if active high is false invert + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0 : 1; + if (inverted) pin_high_level = !pin_high_level; + + uint32_t duty = ledc_get_duty(group, channel); + ledc_channel_config_t ledc_channel; + ledc_channel.speed_mode = group; + ledc_channel.channel = channel; + ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.intr_type = LEDC_INTR_DISABLE; + ledc_channel.gpio_num = pin; + ledc_channel.duty = duty; + ledc_channel.hpoint = 0; + ledc_channel.flags.output_invert = pin_high_level; // 0 is active high, 1 is active low + if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", _channel); + return false; + } + + return true; } +// returns the number of available channels in the group +int _availableGroupChannels(int group){ + if(group == 0) return LEDC_CHANNELS_GROUP0 - group_channels_used[0]; + else if(group == 1) return LEDC_CHANNELS_GROUP1 - group_channels_used[1]; + return 0; +} - +// returns the number of the group that has enough channels available +// returns -1 if no group has enough channels +// +// NOT IMPLEMENTED BUT COULD BE USEFUL +// returns 2 if no group has enough channels but combined they do +int _findGroupWithChannelsAvailable(int no_channels){ + if(no_channels <= _availableGroupChannels(0)) return 0; + if(no_channels <= _availableGroupChannels(1)) return 1; + return -1; +} void* _configure1PWM(long pwm_frequency, const int pinA) { 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 + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 1PWM"); // check if enough channels available - if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - - int ch1 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - + int group = _findGroupWithChannelsAvailable(1); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup in group: ", (group)); + + // configure the channel + group_channels_used[group] += 1; + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1 }, + .channels = { static_cast(group_channels_used[group]) }, + .groups = { (ledc_mode_t)group }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup successful in group: ", (group)); return params; } @@ -84,18 +170,34 @@ 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 SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); - int ch1 = channel_index++; - int ch2 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(2); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[2] = {pinA, pinB}; + for(int i = 0; i < 2; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup successful in group: ", (group)); return params; } @@ -105,20 +207,34 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in 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 SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); - 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); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(3); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[3] = {pinA, pinB, pinC}; + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup successful in group: ", (group)); return params; } @@ -128,54 +244,162 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in 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 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 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + // not enough channels available on any individual group + // check if their combined number is enough (two channels per group) + if(_availableGroupChannels(0) >=2 && _availableGroupChannels(1) >=2){ + group = 2; + SIMPLEFOC_DEBUG("EP32-DRV: WARNING: Not enough available ledc channels for 4pwm in a single group! Using two groups!"); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in groups: 0 and 1!"); + params->groups[2] = (ledc_mode_t)1; + params->groups[3] = (ledc_mode_t)1; + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough available ledc channels for 4pwm!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + params->groups[0] = (ledc_mode_t)group; + params->groups[1] = (ledc_mode_t)group; + params->groups[2] = (ledc_mode_t)group; + params->groups[3] = (ledc_mode_t)group; + } + + + + int pins[4] = {pinA, pinB, pinC, pinD}; + for(int i = 0; i < 4; i++){ + group_channels_used[params->groups[i]]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[params->groups[i]], params->groups[i], pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[params->groups[i]]); + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful!"); return params; } - +void _writeDutyCycle(float dc, void* params, int index){ + ledc_set_duty_with_hpoint(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index], _PWM_RES*dc, _PWM_RES/2.0*(1.0-dc)); + ledc_update_duty(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index]); +} void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); } 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)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); } 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)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); + _writeDutyCycle(dc_c, params, 2); } 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)); + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); } -#endif \ No newline at end of file + +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 50kHz max + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 6PWM"); + SIMPLEFOC_DEBUG("EP32-DRV: WARNING - 6PWM on LEDC is poorly supported and not tested, consider using MCPWM driver instead!"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(6); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)group }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + int high_side_invert = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? false : true; + int low_side_invert = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? true : false; + + int pin_pairs[6][2] = { + {pinA_h, pinA_l}, + {pinB_h, pinB_l}, + {pinC_h, pinC_l} + }; + + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][0], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, high_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i] = static_cast(group_channels_used[group]); + params->groups[2*i] = (ledc_mode_t)group; + + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][1], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, low_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i+1] = static_cast(group_channels_used[group]); + params->groups[2*i+1] = (ledc_mode_t)group; + } + + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup successful in group: ", (group)); + return params; +} + +void _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ + float pwm_h = _constrain(val - dead_time/2.0, 0, 1.0); + float pwm_l = _constrain(val + dead_time/2.0, 0, 1.0); + + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + _writeDutyCycle(0, params, ind_h); + }else{ + _writeDutyCycle(pwm_h, params, ind_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + _writeDutyCycle(0, params, ind_l); + }else{ + _writeDutyCycle(pwm_l, params, ind_l); + } +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPairDutyCycle(params, 0, 1, dc_a, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[0]); + _setPwmPairDutyCycle(params, 2, 3, dc_b, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[1]); + _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); +} + +#endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..e2c621c5 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,226 @@ +#include "esp32_driver_mcpwm.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") + +// function setting the high pwm frequency to the supplied pins +// - DC motor - 1PWM setting +// - hardware specific +void* _configure1PWM(long pwm_frequency, const int pinA) { + 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 + + int group, timer; + if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[1] = {pinA}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware specific +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 + + int group, timer; + int ret = _findBestGroup(2, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 2PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if(ret == 1){ + // configure the 2pwm on only one group + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[2] = {pinA, pinB}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); + }else{ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][1] = {{pinA}, {pinB}}; + for(int i =0; i<2; i++){ + int timer = _findLastTimer(i); //find last created timer in group i + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 1PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + }; + for(int i =0; i<2; i++){ + ret_params->timers[i] = params[i]->timers[0]; + ret_params->oper[i] = params[i]->oper[0]; + ret_params->comparator[i] = params[i]->comparator[0]; + ret_params->generator[i] = params[i]->generator[0]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + +// 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 || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[3] = {pinA, pinB, pinC}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware specific +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 + + int group, timer; + int ret = _findBestGroup(4, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if(ret == 1){ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[4] = {pinA, pinB, pinC, pinD}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); + }else{ + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; + for(int i =0; i<2; i++){ + int timer = _findNextTimer(i); //find next available timer in group i + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 2PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + .timers = {params[0]->timers[0], params[1]->timers[0]}, + .oper = {params[0]->oper[0], params[1]->oper[0]} + }; + for(int i =0; i<4; i++){ + ret_params->comparator[i] = params[(int)floor(i/2)]->comparator[i%2]; + ret_params->generator[i] = params[(int)floor(i/2)]->generator[i%2]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + + +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 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; + return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - DCMotor -1PWM setting +// - hardware specific +void _writeDutyCycle1PWM(float dc_a, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware specific +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware specific +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2b); +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +#if SIMPLEFOC_ESP32_HW_DEADTIME == true + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); + + // set the phase state + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[0], ((ESP32MCPWMDriverParams*)params)->generator[1], phase_state[0]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[2], ((ESP32MCPWMDriverParams*)params)->generator[3], phase_state[1]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[4], ((ESP32MCPWMDriverParams*)params)->generator[5], phase_state[2]); +#else + uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; + float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? dc_a-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? dc_a+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? dc_b-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? dc_b+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[4], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? dc_c-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); +#endif +} +#endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index fecb712c..00000000 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,395 +0,0 @@ -#include "esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -// 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 - {_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 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B - }; - -// 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 -// 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; - 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 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); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - } - _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.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 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (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 = (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.0f / (double)pwm_frequency / (double)(--prescaler + 1); - resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); - - // set 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].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].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 - 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_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); - - // 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 -// - 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 || !_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 = {}; - - // 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); - - 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 - }; - return params; -} - - -// function setting the high pwm frequency to the supplied pins -// - 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) { - 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 = {}; - - // 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_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 - // 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 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); - 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); - - 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 - }; - 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) { - 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 - 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 - esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_4pwm_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; - // 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; - } - - // 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); - - 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 - }; - 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, 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, 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, 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 -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 - 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(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; - - // 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_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_4pwm_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, dead_zone); - // return - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .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, PhaseState *phase_state, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - 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); - _UNUSED(phase_state); -} - -#endif diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h new file mode 100644 index 00000000..dbf48970 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,82 @@ +/* + * This is a private declaration of different MCPWM structures and types. + * It has been copied from: https://github.com/espressif/esp-idf/blob/v5.1.4/components/driver/mcpwm/mcpwm_private.h + * + * extracted by askuric 16.06.2024 + * + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MCPWM_PRIVATE_H +#define MCPWM_PRIVATE_H + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "freertos/FreeRTOS.h" +#include "esp_intr_alloc.h" +#include "esp_heap_caps.h" +#include "esp_pm.h" +#include "soc/soc_caps.h" +#include "hal/mcpwm_hal.h" +#include "hal/mcpwm_types.h" +#include "driver/mcpwm_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct mcpwm_group_t mcpwm_group_t; +typedef struct mcpwm_timer_t mcpwm_timer_t; +typedef struct mcpwm_cap_timer_t mcpwm_cap_timer_t; +typedef struct mcpwm_oper_t mcpwm_oper_t; +typedef struct mcpwm_gpio_fault_t mcpwm_gpio_fault_t; +typedef struct mcpwm_gpio_sync_src_t mcpwm_gpio_sync_src_t; +typedef struct mcpwm_timer_sync_src_t mcpwm_timer_sync_src_t; + +struct mcpwm_group_t { + int group_id; // group ID, index from 0 + int intr_priority; // MCPWM interrupt priority + mcpwm_hal_context_t hal; // HAL instance is at group level + portMUX_TYPE spinlock; // group level spinlock + uint32_t prescale; // group prescale + uint32_t resolution_hz; // MCPWM group clock resolution: clock_src_hz / clock_prescale = resolution_hz + esp_pm_lock_handle_t pm_lock; // power management lock + soc_module_clk_t clk_src; // peripheral source clock + mcpwm_cap_timer_t *cap_timer; // mcpwm capture timers + mcpwm_timer_t *timers[SOC_MCPWM_TIMERS_PER_GROUP]; // mcpwm timer array + mcpwm_oper_t *operators[SOC_MCPWM_OPERATORS_PER_GROUP]; // mcpwm operator array + mcpwm_gpio_fault_t *gpio_faults[SOC_MCPWM_GPIO_FAULTS_PER_GROUP]; // mcpwm fault detectors array + mcpwm_gpio_sync_src_t *gpio_sync_srcs[SOC_MCPWM_GPIO_SYNCHROS_PER_GROUP]; // mcpwm gpio sync array +}; + +typedef enum { + MCPWM_TIMER_FSM_INIT, + MCPWM_TIMER_FSM_ENABLE, +} mcpwm_timer_fsm_t; + +struct mcpwm_timer_t { + int timer_id; // timer ID, index from 0 + mcpwm_group_t *group; // which group the timer belongs to + mcpwm_timer_fsm_t fsm; // driver FSM + portMUX_TYPE spinlock; // spin lock + intr_handle_t intr; // interrupt handle + uint32_t resolution_hz; // resolution of the timer + uint32_t peak_ticks; // peak ticks that the timer could reach to + mcpwm_timer_sync_src_t *sync_src; // timer sync_src + mcpwm_timer_count_mode_t count_mode; // count mode + mcpwm_timer_event_cb_t on_full; // callback function when MCPWM timer counts to peak value + mcpwm_timer_event_cb_t on_empty; // callback function when MCPWM timer counts to zero + mcpwm_timer_event_cb_t on_stop; // callback function when MCPWM timer stops + void *user_data; // user data which would be passed to the timer callbacks +}; + +#ifdef __cplusplus +} +#endif + +#endif + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 15b9c82d..23d94110 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -2,6 +2,12 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP8266") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index d92004e9..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,7 +1,8 @@ + #include "../hardware_api.h" // if the mcu doen't have defiend analogWrite -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(analogWrite) __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index f53eada0..a20b828a 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -3,6 +3,10 @@ #if defined(NRF52_SERIES) +#pragma message("") +#pragma message("SimpleFOC: compiling for NRF52") +#pragma message("") + #define PWM_CLK (16000000) #define PWM_FREQ (40000) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b426f0ed..ad16646a 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -3,6 +3,12 @@ #if defined(TARGET_PORTENTA_H7) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Portenta_H7") +#pragma message("") + + #include "pwmout_api.h" #include "pinDefinitions.h" #include "platform/mbed_critical.h" diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp new file mode 100644 index 00000000..2c9c9b22 --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -0,0 +1,604 @@ + +#include "./renesas.h" + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Renesas (UNO R4)") +#pragma message("") + + + +#include "communication/SimpleFOCDebug.h" +#include "FspTimer.h" + +#define GPT_OPEN (0x00475054ULL) + +/* + We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit) + Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary. + + So each timer channel can handle one half-bridge, using either a single (3-PWM) or + two complimentary PWM signals (6-PWM). + + For 1-PWM through 4-PWM, we need as many channels as PWM signals, and we can use + either output A or B of the timer (we can set the polarity) - but not both. + + For 6-PWM we need 3 channels, and use both outputs A and B of each channel, then + we can do hardware dead-time. + Or we can use seperate channels for high and low side, with software dead-time. + Each phase can be either hardware (1 channel) or software (2 channels) dead-time + individually, they don't all have to be one or the other. + + Selected channels can be started together, so we can keep the phases in sync for + low-side current sensing and software 6-PWM. + + The event system should permit low side current sensing without needing interrupts, + but this will be handled by the current sense driver. + + Supported: + - arbitrary PWM frequencies between 1Hz (minimum we can set with our integer based API) + and around 48kHz (more would be possible but the range will be low) + - PWM range at 24kHz (default) is 1000 + - PWM range at 48kHz is 500 + - polarity setting is supported, in all modes + - phase state setting is supported, in 3-PWM, 6-PWM hardware dead-time and 6-PWM software dead-time + + TODOs: + - change setDutyCycle to use register access for speed + - add event system support for low-side current sensing + - perhaps add support to reserve timers used also in + Arduino Pwm.h code, for compatibility with analogWrite() + - check if there is a better way for phase-state setting + */ + + +// track which channels are used +bool channel_used[GPT_HOWMANY] = { false }; + + +struct RenesasTimerConfig { + timer_cfg_t timer_cfg; + gpt_instance_ctrl_t ctrl; + gpt_extended_cfg_t ext_cfg; + gpt_extended_pwm_cfg_t pwm_cfg; + gpt_io_pin_t duty_pin; +}; + +struct ClockDivAndRange { + timer_source_div_t clk_div; + uint32_t range; +}; + +ClockDivAndRange getClockDivAndRange(uint32_t pwm_frequency, uint8_t timer_channel) { + ClockDivAndRange result; + uint32_t max_count = (timer_channel < GTP32_HOWMANY)? 4294967295 : 65535; + uint32_t freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD); + float range = (float) freq_hz / ((float) pwm_frequency * 2.0f); + if(range / 1.0 < max_count) { + result.range = (uint32_t) (range / 1.0); + result.clk_div = TIMER_SOURCE_DIV_1; + } + else if (range / 2.0 < max_count) { + result.range = (uint32_t) (range / 2.0); + result.clk_div = TIMER_SOURCE_DIV_2; + } + else if(range / 4.0 < max_count) { + result.range = (uint32_t) (range / 4.0); + result.clk_div = TIMER_SOURCE_DIV_4; + } + else if(range / 8.0 < max_count) { + result.range = (uint32_t) (range / 8.0 ); + result.clk_div = TIMER_SOURCE_DIV_8; + } + else if(range / 16.0 < max_count) { + result.range = (uint32_t) (range / 16.0 ); + result.clk_div = TIMER_SOURCE_DIV_16; + } + else if (range / 32.0 < max_count) { + result.range = (uint32_t) (range / 32.0 ); + result.clk_div = TIMER_SOURCE_DIV_32; + } + else if(range / 64.0 < max_count) { + result.range = (uint32_t) (range / 64.0 ); + result.clk_div = TIMER_SOURCE_DIV_64; + } + else if(range / 128.0 < max_count) { + result.range = (uint32_t) (range / 128.0 ); + result.clk_div = TIMER_SOURCE_DIV_128; + } + else if(range / 256.0 < max_count) { + result.range = (uint32_t) (range / 256.0 ); + result.clk_div = TIMER_SOURCE_DIV_256; + } + else if(range / 512.0 < max_count) { + result.range = (uint32_t) (range / 512.0 ); + result.clk_div = TIMER_SOURCE_DIV_512; + } + else if(range / 1024.0 < max_count) { + result.range = (uint32_t) (range / 1024.0 ); + result.clk_div = TIMER_SOURCE_DIV_1024; + } + else { + SimpleFOCDebug::println("DRV: PWM frequency too low"); + } + return result; +}; + + +bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool active_high, bool complementary = false, bool complementary_active_high = true) { + uint8_t pin = params->pins[index]; + uint8_t pin_C; + std::array pinCfgs = getPinCfgs(pin, PIN_CFG_REQ_PWM); + std::array pinCfgs_C; + if(pinCfgs[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs[0])) { + SIMPLEFOC_DEBUG("DRV: AGT timer not supported"); + return false; + } + // get the timer channel + uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]); + // check its not used + if (channel_used[timer_channel]) { + SIMPLEFOC_DEBUG("DRV: channel in use: ", timer_channel); + return false; + } + + if (complementary) { + pin_C = params->pins[index+1]; + pinCfgs_C = getPinCfgs(pin_C, PIN_CFG_REQ_PWM); + if(pinCfgs_C[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin_C); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs_C[0]) || GET_CHANNEL(pinCfgs_C[0])!=timer_channel) { + SIMPLEFOC_DEBUG("DRV: comp. channel different"); + return false; + } + } + TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; + if (complementary) { + TimerPWMChannel_t pwm_output_C = IS_PWM_ON_A(pinCfgs_C[0]) ? CHANNEL_A : CHANNEL_B; + if (pwm_output_C != CHANNEL_A || pwm_output != CHANNEL_B) { + SIMPLEFOC_DEBUG("DRV: output A/B mismatch"); + return false; + } + } + + // configure GPIO pin + fsp_err_t err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if ((err == FSP_SUCCESS) && complementary) + err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin_C].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin config failed"); + return false; + } + + + // configure timer channel - frequency / top value + ClockDivAndRange timings = getClockDivAndRange(params->pwm_frequency, timer_channel); + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("---PWM Config---"); + SimpleFOCDebug::println("DRV: pwm pin: ", pin); + if (complementary) + SimpleFOCDebug::println("DRV: compl. pin: ", pin_C); + SimpleFOCDebug::println("DRV: pwm channel: ", timer_channel); + SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println(complementary?"A+B":((pwm_output==CHANNEL_A)?"A":"B")); + SimpleFOCDebug::println("DRV: pwm freq: ", (int)params->pwm_frequency); + SimpleFOCDebug::println("DRV: pwm range: ", (int)timings.range); + SimpleFOCDebug::println("DRV: pwm clkdiv: ", timings.clk_div); + #endif + + RenesasTimerConfig* t = new RenesasTimerConfig(); + // configure timer channel - count mode + t->timer_cfg.channel = timer_channel; + t->timer_cfg.mode = TIMER_MODE_TRIANGLE_WAVE_SYMMETRIC_PWM; + t->timer_cfg.source_div = timings.clk_div; + t->timer_cfg.period_counts = timings.range; + t->timer_cfg.duty_cycle_counts = 0; + t->timer_cfg.p_callback = nullptr; + t->timer_cfg.p_context = nullptr; + t->timer_cfg.p_extend = &(t->ext_cfg); + t->timer_cfg.cycle_end_ipl = BSP_IRQ_DISABLED; + t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR; + + t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg); + t->ext_cfg.capture_a_ipl = BSP_IRQ_DISABLED; + t->ext_cfg.capture_a_irq = FSP_INVALID_VECTOR; + t->ext_cfg.capture_b_ipl = BSP_IRQ_DISABLED; + t->ext_cfg.capture_b_irq = FSP_INVALID_VECTOR; + t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED; + t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR; + t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0; + t->pwm_cfg.output_disable = GPT_OUTPUT_DISABLE_NONE; + t->pwm_cfg.adc_trigger = GPT_ADC_TRIGGER_NONE; + t->pwm_cfg.dead_time_count_up = 0; + t->pwm_cfg.dead_time_count_down = 0; + t->pwm_cfg.adc_a_compare_match = 0; + t->pwm_cfg.adc_b_compare_match = 0; + t->pwm_cfg.interrupt_skip_source = GPT_INTERRUPT_SKIP_SOURCE_NONE; + t->pwm_cfg.interrupt_skip_count = GPT_INTERRUPT_SKIP_COUNT_0; + t->pwm_cfg.interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE; + t->pwm_cfg.gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + t->pwm_cfg.gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + // configure dead-time if both outputs are used + if (complementary) { + uint32_t dt = params->dead_zone * timings.range; + t->pwm_cfg.dead_time_count_up = dt; + t->pwm_cfg.dead_time_count_down = dt; + } + + // configure timer channel - outputs and polarity + t->ext_cfg.gtior_setting.gtior = 0L; + if (!complementary) { + if(pwm_output == CHANNEL_A) { + t->duty_pin = GPT_IO_PIN_GTIOCA; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01; + // t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0; + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCB; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtioca.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; + } + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (!complementary_active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = !complementary_active_high ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; + } + memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); + err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); + if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { + SIMPLEFOC_DEBUG("DRV: open failed"); + return false; + } + if (err == FSP_ERR_ALREADY_OPEN) { + SimpleFOCDebug::println("DRV: timer already open"); + return false; + } + + err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: period set failed"); + return false; + } + err = R_GPT_OutputEnable(&(t->ctrl), t->duty_pin); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin enable failed"); + return false; + } + + channel_used[timer_channel] = true; + params->timer_config[index] = t; + params->channels[index] = timer_channel; + if (complementary) { + params->timer_config[index+1] = t; + params->channels[index+1] = timer_channel; + } + + return true; +} + + +// start the timer channels for the motor, synchronously +bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { + uint32_t mask = 0; + for (int i = 0; i < num_channels; i++) { + // RenesasTimerConfig* t = params->timer_config[i]; + // if (R_GPT_Start(&(t->ctrl)) != FSP_SUCCESS) { + // SIMPLEFOC_DEBUG("DRV: timer start failed"); + // return false; + // } + mask |= (1 << params->channels[i]); +#if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: starting timer: ", params->channels[i]); +#endif + } + params->timer_config[0]->ctrl.p_reg->GTSTR |= mask; + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: timers started"); + #endif + return true; +} + + +// check if the given pins are on the same timer channel +bool isHardware6Pwm(const int pin1, const int pin2) { + std::array pinCfgs1 = getPinCfgs(pin1, PIN_CFG_REQ_PWM); + std::array pinCfgs2 = getPinCfgs(pin2, PIN_CFG_REQ_PWM); + if(pinCfgs1[0] == 0 || pinCfgs2[0] == 0) + return false; + if (IS_PIN_AGT_PWM(pinCfgs1[0]) || IS_PIN_AGT_PWM(pinCfgs2[0])) + return false; + uint8_t timer_channel1 = GET_CHANNEL(pinCfgs1[0]); + uint8_t timer_channel2 = GET_CHANNEL(pinCfgs2[0]); + return timer_channel1==timer_channel2; +} + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 1); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (!success) + success &= startTimerChannels(params, 2); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; params->pins[2] = pinC; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 3); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pin1A; params->pins[1] = pin1B; params->pins[2] = pin2A; params->pins[3] = pin2B; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 4); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +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){ + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA_h; params->pins[1] = pinA_l; params->pins[2] = pinB_h; params->pins[3] = pinB_l; params->pins[4] = pinC_h; params->pins[5] = pinC_l; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + params->dead_zone = (dead_zone==NOT_SET)?RENESAS_DEFAULT_DEAD_ZONE:dead_zone; + + bool success = true; + if (isHardware6Pwm(pinA_h, pinA_l)) + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); // reverse polarity on low side gives desired active high/low behaviour + } + + if (isHardware6Pwm(pinB_h, pinB_l)) + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } + + if (isHardware6Pwm(pinC_h, pinC_l)) + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 5, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } + + if (success) + success = startTimerChannels(params, 6); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + + + +void _writeDutyCycle1PWM(float dc_a, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_1a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_1b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_2a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + duty_cycle_counts = (uint32_t)(dc_2b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + + +void _setSinglePhaseState(RenesasTimerConfig* hi, RenesasTimerConfig* lo, PhaseState state) { + gpt_gtior_setting_t gtior; + gtior.gtior = hi->ctrl.p_reg->GTIOR; + bool on = (state==PHASE_ON) || (state==PHASE_HI); + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA_AND_GTIOCB) { + bool ch = false; + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + ch = true; + } // B is high side + on = (state==PHASE_ON) || (state==PHASE_LO); + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + ch = true; + } + if (ch) + hi->ctrl.p_reg->GTIOR = gtior.gtior; + return; + } + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (hi->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + + gtior.gtior = lo->ctrl.p_reg->GTIOR; + on = (state==PHASE_ON) || (state==PHASE_LO); + if (lo->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (lo->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + +} + + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + RenesasTimerConfig* t1 = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + uint32_t dt = (uint32_t)(((RenesasHardwareDriverParams*)params)->dead_zone * (float)(t->timer_cfg.period_counts)); + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + bool hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[0] == ((RenesasHardwareDriverParams*)params)->channels[1]; + uint32_t dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[0]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[2] == ((RenesasHardwareDriverParams*)params)->channels[3]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[1]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[4]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[5]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[4] == ((RenesasHardwareDriverParams*)params)->channels[5]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[2]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/renesas/renesas.h b/src/drivers/hardware_specific/renesas/renesas.h new file mode 100644 index 00000000..91bacdce --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.h @@ -0,0 +1,28 @@ +#pragma once + + +#include "../../hardware_api.h" + + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + +// uncomment to enable debug output from Renesas driver +// can set this as build-flag in Arduino IDE or PlatformIO +#define SIMPLEFOC_RENESAS_DEBUG + +#define RENESAS_DEFAULT_PWM_FREQUENCY 24000 +#define RENESAS_DEFAULT_DEAD_ZONE 0.05f + +struct RenesasTimerConfig; + +typedef struct RenesasHardwareDriverParams { + uint8_t pins[6]; + uint8_t channels[6]; + RenesasTimerConfig* timer_config[6]; + long pwm_frequency; + float dead_zone; +} RenesasHardwareDriverParams; + + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index a2970c72..6afbf345 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -2,17 +2,31 @@ /** * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. */ + +#include "./rp2040_mcu.h" + + #if defined(TARGET_RP2040) + +#pragma message("") +#pragma message("SimpleFOC: compiling for RP2040") +#pragma message("") + +#if !defined(SIMPLEFOC_DEBUG_RP2040) #define SIMPLEFOC_DEBUG_RP2040 +#endif #include "../../hardware_api.h" -#include "./rp2040_mcu.h" #include "hardware/pwm.h" +#include "hardware/clocks.h" +#if defined(USE_ARDUINO_PINOUT) +#include +#endif #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 -#define _PWM_FREQUENCY_MIN 5000 +#define _PWM_FREQUENCY_MIN 1 @@ -23,30 +37,44 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { +void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + #if defined(USE_ARDUINO_PINOUT) + uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? + #else + uint pin = (uint)pin_nr; + #endif 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; - if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 - if (wrapvalue > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 + uint32_t sysclock_hz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS) * 1000; + uint32_t factor = 4096 * 2 * pwm_frequency; + uint32_t div = sysclock_hz / factor; + if (sysclock_hz % factor !=0) div+=1; + if (div < 16) div = 16; + uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); - SimpleFOCDebug::print(pin); + SimpleFOCDebug::print((int)pin); SimpleFOCDebug::print(" slice "); SimpleFOCDebug::print((int)slice); SimpleFOCDebug::print(" channel "); SimpleFOCDebug::print((int)chan); SimpleFOCDebug::print(" frequency "); SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" divisor "); + SimpleFOCDebug::print((int)(div>>4)); + SimpleFOCDebug::print("."); + SimpleFOCDebug::print((int)(div&0xF)); SimpleFOCDebug::print(" top value "); - SimpleFOCDebug::println(wrapvalue); + SimpleFOCDebug::println((int)wrapvalue); #endif + if (wrapvalue < 999) + SimpleFOCDebug::println("Warning: PWM resolution is low."); + pwm_set_clkdiv_int_frac(slice, div>>4, div&0xF); + pwm_set_phase_correct(slice, true); pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; if (invert) { diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h index c3b4fe51..bbfb3873 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -2,6 +2,8 @@ #pragma once +#include "Arduino.h" + #if defined(TARGET_RP2040) diff --git a/src/drivers/hardware_specific/samd/samd21_mcu.cpp b/src/drivers/hardware_specific/samd/samd21_mcu.cpp index d9bb5b9a..d59a3098 100644 --- a/src/drivers/hardware_specific/samd/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd21_mcu.cpp @@ -7,6 +7,10 @@ #ifdef _SAMD21_ +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD21") +#pragma message("") + #ifndef TCC3_CH0 #define TCC3_CH0 NOT_ON_TIMER diff --git a/src/drivers/hardware_specific/samd/samd51_mcu.cpp b/src/drivers/hardware_specific/samd/samd51_mcu.cpp index 4bdf036b..71bf0b81 100644 --- a/src/drivers/hardware_specific/samd/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd51_mcu.cpp @@ -6,6 +6,11 @@ #if defined(_SAMD51_)||defined(_SAME51_) +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD51/SAME51") +#pragma message("") + + // 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. diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index d16808f6..4009281e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,8 +4,11 @@ #if defined(_STM32_DEF_) +#define SIMPLEFOC_STM32_DEBUG +#pragma message("") +#pragma message("SimpleFOC: compiling for STM32") +#pragma message("") -//#define SIMPLEFOC_STM32_DEBUG #ifdef SIMPLEFOC_STM32_DEBUG void printTimerCombination(int numPins, PinMap* timers[], int score); @@ -23,7 +26,15 @@ PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; +bool _getPwmState(void* params) { + // assume timers are synchronized and that there's at least one timer + HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; + TIM_HandleTypeDef* htim = pHT->getHandle(); + + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); + return dir; +} // setting pwm to hardware pin - instead analogWrite() @@ -200,17 +211,171 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } } -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // TODO - sart each timer only once - // sart timers - for (int i=0; i < timer_num; i++) { - if(timers_to_start[i] == NP) return; - timers_to_start[i]->resume(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + +#if defined(STM32G4xx) +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + #endif + #if defined(TIM5) && defined(LL_TIM_TS_ITR4) + else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + #endif + #if defined(TIM8) && defined(LL_TIM_TS_ITR5) + else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + #endif + return -1; +} +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) + +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + #if defined(TIM2) + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { + return -1; +} +#endif + +void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { + uint32_t max_frequency = 0; + uint32_t min_frequency = UINT32_MAX; + for (size_t i=0; igetTimerClkFreq(); + if (freq > max_frequency) { + max_frequency = freq; + } else if (freq < min_frequency) { + min_frequency = freq; + } + } + if (max_frequency==min_frequency) return; + uint32_t overflow_value = min_frequency/pwm_frequency; + for (size_t i=0; igetTimerClkFreq()/min_frequency; + #ifdef SIMPLEFOC_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); + SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); #endif + timers[i]->setPrescaleFactor(prescale_factor); + timers[i]->setOverflow(overflow_value,TICK_FORMAT); + timers[i]->refresh(); } } @@ -218,7 +383,7 @@ void _alignTimersNew() { int numTimers = 0; HardwareTimer *timers[numTimerPinsUsed]; - // reset timer counters + // find the timers used for (int i=0; iperipheral); HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); @@ -233,10 +398,66 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + #endif + + // see if there is more then 1 timers used for the pwm + // if yes, try to align timers + if(numTimers > 1){ + // find the master timer + int16_t master_index = -1; + int triggerEvent = -1; + for (int i=0; igetHandle()->Instance)) { + // check if timer already configured in TRGO update mode (used for ADC triggering) + // in that case we should not change its TRGO configuration + if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; + // check if the timer has the supported internal trigger for other timers + for (int slave_i=0; slave_igetHandle()->Instance))); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode + LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + // configure other timers to get the input trigger from the master timer + for (int slave_index=0; slave_index < numTimers; slave_index++) { + if (slave_index == master_index) + continue; + // Configure the slave timer to be triggered by the master enable signal + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); + LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + } + } + } + // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + // timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -250,6 +471,20 @@ void _alignTimersNew() { +// align the timers to end the init +void _startTimers(HardwareTimer **timers_to_start, int timer_num) +{ + // // TODO - start each timer only once + // // start timers + // for (int i=0; i < timer_num; i++) { + // if(timers_to_start[i] == NP) return; + // timers_to_start[i]->resume(); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + // #endif + // } + _alignTimersNew(); +} // configure hardware 6pwm for a complementary pair of channels @@ -287,6 +522,11 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* // 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); + if (dead_time>255) dead_time = 255; + if (dead_time==0 && dead_zone>0) { + dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large + SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max"); + } LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); @@ -531,7 +771,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // allign the timers + // align the timers _alignTimersNew(); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); @@ -570,6 +810,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer *timers[2] = {HT1, HT2}; + syncTimerFrequency(pwm_frequency, timers, 2); // allign the timers _alignPWMTimers(HT1, HT2, HT2); @@ -589,6 +831,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +TIM_MasterConfigTypeDef sMasterConfig; +TIM_SlaveConfigTypeDef sSlaveConfig; // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -611,7 +855,10 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - + + HardwareTimer *timers[3] = {HT1, HT2, HT3}; + syncTimerFrequency(pwm_frequency, timers, 3); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -654,6 +901,8 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); + HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; + syncTimerFrequency(pwm_frequency, timers, 4); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); @@ -751,6 +1000,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + syncTimerFrequency(pwm_frequency, timers, 6); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -936,8 +1187,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif - - - - #endif diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..411c43b2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -28,5 +28,8 @@ typedef struct STM32DriverParams { void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); +// timer query functions +bool _getPwmState(void* params); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp index 742d79df..fe145273 100644 --- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -3,14 +3,17 @@ // if defined // - Teensy 3.0 MK20DX128 // - Teensy 3.1/3.2 MK20DX256 -// - Teensy 3.5 MK20DX128 // - Teensy LC MKL26Z64 // - Teensy 3.5 MK64FX512 // - Teensy 3.6 MK66FX1M0 #if defined(__arm__) && defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)) -// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 3.x") +#pragma message("") + +// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c#L627 #if defined(__MK20DX128__) #define FTM0_CH0_PIN 22 #define FTM0_CH1_PIN 23 @@ -112,7 +115,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i } } - #ifdef FTM3_SC // if the board has FTM3 timer +#ifdef FTM3_CH0_PIN // if the board has FTM3 timer if((Ah == FTM3_CH0_PIN && Al == FTM3_CH1_PIN) || (Ah == FTM3_CH2_PIN && Al == FTM3_CH3_PIN) || (Ah == FTM3_CH4_PIN && Al == FTM3_CH5_PIN) ){ @@ -130,7 +133,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i } } } - #endif +#endif #ifdef SIMPLEFOC_TEENSY_DEBUG SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Pins not on timers FTM0 or FTM3!"); @@ -206,7 +209,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - Stepper motor - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); // transform duty cycle from [0,1] to [0,255] // phase A analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index c961ad06..47108447 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,4 +1,3 @@ -#include "teensy_mcu.h" #include "teensy4_mcu.h" #include "../../../communication/SimpleFOCDebug.h" @@ -7,10 +6,150 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// half_cycle of the PWM variable -int half_cycle = 0; +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + +// #define SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule < 0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm){ + if(flexpwm == &IMXRT_FLEXPWM1) return 1; + if(flexpwm == &IMXRT_FLEXPWM2) return 2; + if(flexpwm == &IMXRT_FLEXPWM3) return 3; + if(flexpwm == &IMXRT_FLEXPWM4) return 4; + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a null-pointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + // function which finds the flexpwm instance for a pair of pins // if they do not belong to the same timer it returns a nullpointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ @@ -92,11 +231,33 @@ int get_submodule(uint8_t pin, uint8_t pin1){ } +// function which finds the channel for flexpwm timer pin +// 0 - X +// 1 - A +// 2 - B +int get_channel(uint8_t pin){ + const struct pwm_pin_info_struct *info; + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[90]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); + SIMPLEFOC_DEBUG(s); +#endif + return info->channel; +} + // function which finds the timer submodule for a pair of pins // if they do not belong to the same submodule it returns a -1 int get_inverted_channel(uint8_t pin, uint8_t pin1){ - const struct pwm_pin_info_struct *info; if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; @@ -106,10 +267,8 @@ int get_inverted_channel(uint8_t pin, uint8_t pin1){ return -1; } - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); if (ch1 != 1) { #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -139,7 +298,7 @@ return ch2; // can configure sync, prescale and B inversion. // sets the desired frequency of the PWM // sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone ) +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) { int submodule_mask = 1 << submodule ; flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running @@ -161,33 +320,98 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque } // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); + int half_cycle = int(newdiv/2.0f); int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% int mid_pwm = int((half_cycle)/2.0f); + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + // setup the timer // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match. flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0 ; + flexpwm->SM[submodule].VAL0 = 0; flexpwm->SM[submodule].VAL1 = half_cycle ; flexpwm->SM[submodule].VAL2 = -mid_pwm ; flexpwm->SM[submodule].VAL3 = +mid_pwm ; - // flexpwm->SM[submodule].VAL4 = -mid_pwm ; - // flexpwm->SM[submodule].VAL5 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// Helper to set up a FlexPWM submodule. +// can configure sync, prescale +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_timer_submodule (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + int half_cycle = int(newdiv/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = 0 ; + flexpwm->SM[submodule].DTCNT1 = 0 ; + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running } +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, int channel) +{ + int submodule_mask = 1 << submodule ; + + if(channel==1) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + else if(channel==2) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + // staring the PWM on A and B channels of the submodule void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) { @@ -201,20 +425,15 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - + int half_cycle = int(flexpwm->SM[submodule].VAL1); int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - flexpwm->SM[submodule].VAL2 = count_pwm; // A on - flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off - // flexpwm->SM[submodule].VAL4 = - count_pwm ; // B off (assuming B inverted) - // flexpwm->SM[submodule].VAL5 = + count_pwm ; // B on + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + _UNUSED(phase_state); + write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a); + write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b); + write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c); +} + +void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty) +{ + uint16_t mask = 1 << submodule; + uint32_t half_cycle = p->SM[submodule].VAL1; + int mid_pwm = int((half_cycle)/2.0f); + int cval = int(mid_pwm*(duty*2-1)) + mid_pwm; + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = half_cycle - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL2 = -cval; + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL4 = -cval; + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#endif \ No newline at end of file +#ifdef SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configureCenterAligned3PMW(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 + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + int channelA = get_channel(pinA); + int channelB = get_channel(pinB); + int channelC = get_channel(pinC); + + + // if pins belong to the flextimers and they only use submodules A and B + // we can configure the center-aligned pwm + if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B channels - Configuring center-aligned pwm!"); + #endif + + // Configure FlexPWM units + setup_pwm_timer_submodule (flexpwmA, submoduleA, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmB, submoduleB, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmC, submoduleC, false, pwm_frequency) ; // this is the master, internally synced + delayMicroseconds (100) ; + + + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-CS: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-CS: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif + + // // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {channelA, channelB, channelC}, + } + }; + + startup_pwm_pair (flexpwmA, submoduleA, channelA) ; + startup_pwm_pair (flexpwmB, submoduleB, channelB) ; + startup_pwm_pair (flexpwmC, submoduleC, channelC) ; + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; + }else{ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B channels - cannot configure center-aligned pwm!"); + #endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + write_pwm_on_pin (p->flextimers[0], p->submodules[0], p->channels[0], dc_a); + write_pwm_on_pin (p->flextimers[1], p->submodules[1], p->channels[1], dc_b); + write_pwm_on_pin (p->flextimers[2], p->submodules[2], p->channels[2], dc_c); +} + +#endif + +#endif diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index 5e384623..aed64826 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + #include "teensy_mcu.h" // if defined @@ -9,7 +12,7 @@ typedef struct Teensy4DriverParams { IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; - long pwm_frequency; + int channels[6]; float dead_zone; } Teensy4DriverParams; @@ -105,6 +108,18 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm); +// find the trigger TRG0 for the given timer and submodule +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// find the external trigger for the given timer and submodule +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// function to connecting the triggers +void xbar_connect(unsigned int input, unsigned int output); +// function to initialize the xbar +void xbar_init(); + #endif +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index dcfa3e15..196f07fd 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -2,6 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#include "../../../communication/SimpleFOCDebug.h" + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); @@ -11,14 +13,15 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure1PWM(long pwm_frequency, const int pinA) { 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); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -26,38 +29,54 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific 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 { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm +__attribute__((weak)) void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific 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 - }; + + // try configuring center aligned pwm + void* p = _configureCenterAligned3PMW(pwm_frequency, pinA, pinB, pinC); + if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ + return p; // if center aligned pwm is available return the params + }else{ // if center aligned pwm is not available use fast pwm + SIMPLEFOC_DEBUG("TEENSY-DRV: Configuring 3PWM with fast pwm. Please consider using center aligned pwm for better performance!"); + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; return params; + } } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific 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 @@ -65,9 +84,10 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB, pinC, pinD }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -75,42 +95,57 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_b); +} + +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm implementation of this function +__attribute__((weak)) void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(params); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,255] - 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); + + TeensyDriverParams* p = (TeensyDriverParams*)params; + if(p->additional_params != nullptr){ + _writeCenterAligned3PMW(dc_a, dc_b, dc_c, p); + }else{ + // transform duty cycle from [0,1] to [0,255] + analogWrite(p->pins[0], 255.0f*dc_a); + analogWrite(p->pins[1], 255.0f*dc_b); + analogWrite(p->pins[2], 255.0f*dc_c); + } } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific 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(((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); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((TeensyDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((TeensyDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.h b/src/drivers/hardware_specific/teensy/teensy_mcu.h index 7956ea90..266f4b69 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + #include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +11,17 @@ // debugging output #define SIMPLEFOC_TEENSY_DEBUG +typedef struct TeensyDriverParams { + int pins[6] = {(int)NOT_SET}; + long pwm_frequency; + void* additional_params; +} TeensyDriverParams; + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin); +void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params); + +#endif #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 96057d82..fa4b8e73 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -99,29 +99,23 @@ void Encoder::handleIndex() { } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { - // do nothing for Encoder + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + noInterrupts(); + angle_prev_ts = pulse_timestamp; + long copy_pulse_counter = pulse_counter; + interrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = copy_pulse_counter / (int)cpr; + angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); } /* Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getAngle(); -} -// 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(){ - return _2PI * (pulse_counter) / ((float)cpr); -} -double Encoder::getPreciseAngle(){ - return _2PI * (pulse_counter) / ((double)cpr); -} -int32_t Encoder::getFullRotations(){ - return pulse_counter / (int)cpr; + return _2PI * (pulse_counter) / ((float)cpr); } @@ -131,6 +125,11 @@ int32_t Encoder::getFullRotations(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + noInterrupts(); + long copy_pulse_counter = pulse_counter; + long copy_pulse_timestamp = pulse_timestamp; + interrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -139,8 +138,8 @@ float Encoder::getVelocity(){ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6f; - long dN = pulse_counter - prev_pulse_counter; + float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f; + long dN = copy_pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) // dN - impulses received @@ -161,7 +160,7 @@ float Encoder::getVelocity(){ prev_timestamp_us = timestamp_us; // save velocity calculation variables prev_Th = Th; - prev_pulse_counter = pulse_counter; + prev_pulse_counter = copy_pulse_counter; return velocity; } diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 91427a83..af6a5ab6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,12 +61,8 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getAngle() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; virtual void update() override; /** diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 7d1d9f64..64c5e48c 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -42,23 +42,21 @@ 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; - } + if (new_hall_state == hall_state) return; + + long new_pulse_timestamp = _micros(); 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) { + int8_t electric_sector_dif = new_electric_sector - electric_sector; + if (electric_sector_dif > 3) { //underflow direction = Direction::CCW; electric_rotations += direction; - } else if (new_electric_sector - electric_sector < (-3)) { + } else if (electric_sector_dif < (-3)) { //overflow direction = Direction::CW; electric_rotations += direction; @@ -94,8 +92,24 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { -float HallSensor::getSensorAngle() { - return getAngle(); +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void HallSensor::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + if (use_interrupt){ + noInterrupts(); + }else{ + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + } + + angle_prev_ts = pulse_timestamp; + long last_electric_rotations = electric_rotations; + int8_t last_electric_sector = electric_sector; + if (use_interrupt) interrupts(); + angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -104,8 +118,8 @@ float HallSensor::getSensorAngle() { Shaft angle calculation TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getMechanicalAngle() { - return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; +float HallSensor::getSensorAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } /* @@ -113,38 +127,18 @@ float HallSensor::getMechanicalAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ + noInterrupts(); + long last_pulse_timestamp = pulse_timestamp; long last_pulse_diff = pulse_diff; - if (last_pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > last_pulse_diff) ) { // last velocity isn't accurate if too old + interrupts(); + if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old return 0; } else { - float vel = direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); - // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 - if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero - return vel; + return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); } } - - -float HallSensor::getAngle() { - return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; -} - - -double HallSensor::getPreciseAngle() { - 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(){ @@ -163,7 +157,7 @@ void HallSensor::init(){ } // init hall_state - A_active= digitalRead(pinA); + A_active = digitalRead(pinA); B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); @@ -182,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); + + use_interrupt = true; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index d6b4493a..94053e0f 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -47,23 +47,23 @@ class HallSensor: public Sensor{ int pinA; //!< HallSensor hardware pin A int pinB; //!< HallSensor hardware pin B int pinC; //!< HallSensor hardware pin C + int use_interrupt; //!< True if interrupts have been attached // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation + /** Interrupt-safe update */ + void update() override; /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; + Direction old_direction; void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index af93b8cc..2b3db0c2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -16,6 +16,14 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .data_start_bit = 15 }; +/** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s MT6701_I2C = { + .chip_address = 0x06, + .bit_resolution = 14, + .angle_register = 0x03, + .data_start_bit = 15 +}; + // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address @@ -28,12 +36,13 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) - cpr = pow(2, _bit_resolution); + cpr = _powtwo(_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 + // MT6701 uses 0..5 LSB and 9..15 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -48,7 +57,7 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ // 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); + cpr = _powtwo(config.bit_resolution); int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; @@ -58,6 +67,10 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ wire = &Wire; } +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; @@ -95,7 +108,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // notify the device that is aboout to be read wire->beginTransmission(chip_address); wire->write(angle_reg_msb); - wire->endTransmission(false); + currWireError = wire->endTransmission(false); // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); @@ -107,6 +120,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB + // MT6701 uses 0..5 LSB and 6..13 MSB readValue = ( readArray[1] & lsb_mask ); readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); return readValue; diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index fa7ec96b..f8b189fa 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -51,6 +51,9 @@ class MagneticSensorI2C: public Sensor{ /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin , byte scl_pin ); + /** current error code from Wire endTransmission() call **/ + uint8_t currWireError = 0; + private: float cpr; //!< Maximum range of the magnetic sensor uint16_t lsb_used; //!< Number of bits used in LSB register diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 02798ad3..04b7cc75 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -56,10 +56,21 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + pulse_timestamp = _micros(); this->Sensor::init(); // call base class init } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void MagneticSensorPWM::update() { + if (is_interrupt_based) + noInterrupts(); + Sensor::update(); + angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication + if (is_interrupt_based) + interrupts(); +} + // get current angle (rad) float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor @@ -73,7 +84,8 @@ float MagneticSensorPWM::getSensorAngle(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way - pulse_length_us = pulseIn(pinPWM, HIGH); + pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse + pulse_length_us = pulseIn(pinPWM, HIGH, timeout_us); // 1200us timeout, should this be configurable? } return pulse_length_us; } @@ -84,7 +96,10 @@ void MagneticSensorPWM::handlePWM() { unsigned long now_us = _micros(); // if falling edge, calculate the pulse length - if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; + if (!digitalRead(pinPWM)) { + pulse_length_us = now_us - last_call_us; + pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp + } // save the currrent timestamp for the next call last_call_us = now_us; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 77e82459..a5fd7e6e 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -32,6 +32,9 @@ class MagneticSensorPWM: public Sensor{ void init(); int pinPWM; + + // Interrupt-safe update + void update() override; // get current angle (rad) float getSensorAngle() override; @@ -41,6 +44,8 @@ class MagneticSensorPWM: public Sensor{ void enableInterrupt(void (*doPWM)()); unsigned long pulse_length_us; + unsigned int timeout_us = 1200; + private: // raw count (typically in range of 0-1023) int raw_count; @@ -62,6 +67,7 @@ class MagneticSensorPWM: public Sensor{ // time tracking variables unsigned long last_call_us; // unsigned long pulse_length_us; + unsigned long pulse_timestamp; }; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 7341c89a..52febc38 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -31,13 +31,13 @@ MagneticSensorSPIConfig_s MA730_SPI = { // 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){ +MagneticSensorSPI::MagneticSensorSPI(int cs, int _bit_resolution, int _angle_register){ 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) - cpr = pow(2,_bit_resolution); + cpr = _powtwo(_bit_resolution); spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; @@ -52,7 +52,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int 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) - cpr = pow(2, config.bit_resolution); + cpr = _powtwo(config.bit_resolution); spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index a77e7698..03ebde44 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -30,7 +30,7 @@ class MagneticSensorSPI: public Sensor{ * @param bit_resolution sensor resolution bit number * @param angle_register (optional) angle read register - default 0x3FFF */ - MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0); + MagneticSensorSPI(int cs, int bit_resolution, int angle_register = 0); /** * MagneticSensorSPI class constructor * @param config SPI config