diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 00000000..3d352a1b --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,29 @@ +--- +name: Bug report +about: Create a report to help us improve +title: "[BUG]" +labels: possible bug +assignees: '' + +--- + +This is a simplified template, feel free to change it if it does not fit your case. + +**Describe the bug** +A clear and concise description of what the bug is. + +**Describe the hardware setup** +For us it is very important to know what is the hardware setup you're using in order to be able to help more directly +- Which motor +- Which driver +- Which microcontroller +- Which position sensor +- Current sensing used? + +**IDE you are using** +- Arduino IDE +- Platformio +- Something else + +**Tried the Getting started guide? - if applicable** +Have you tried the getting started guide and at which step are you blocked in diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 00000000..df558478 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: "[FEATURE]" +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +Description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +Description of what you want to happen. + +**Describe alternatives you've considered** +Description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. 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 6171ab77..00000000 --- a/.github/workflows/ccpp.yml +++ /dev/null @@ -1,65 +0,0 @@ -name: Library Compile -on: push -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:samd:nano_33_iot # samd21 - - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esm32 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - arduino:mbed_rp2040:pico # rpi pico - - include: - - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples - sketch-names: '**.ino' - required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1_position_control - - - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example - sketch-names: single_full_control_example.ino - - - 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:esp32doit-devkit-v1 # esp32 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino - - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino - - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # one full example - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino - - - - # 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 }} 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/CITATION.cff b/CITATION.cff new file mode 100644 index 00000000..ec37686e --- /dev/null +++ b/CITATION.cff @@ -0,0 +1,48 @@ +cff-version: 1.0.0 +message: "If you use this software, please cite it as below." +authors: +- family-names: "Skuric" + given-names: "Antun" + orcid: "https://orcid.org/0000-0002-3323-4482" +- family-names: "Bank" + given-names: "Hasan Sinan" + orcid: "https://orcid.org/0000-0002-0626-2664" +- family-names: "Unger" + given-names: "Richard" +- family-names: "Williams" + given-names: "Owen" +- family-names: "González-Reyes" + given-names: "David" + orcid: "https://orcid.org/0000-0002-1535-3007" +title: "SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors" +version: 2.2.2 +doi: 10.21105/joss.04232 +date-released: 2022-06-26 +url: "https://github.com/simplefoc/Arduino-FOC" + +preferred-citation: + type: article + authors: + - family-names: "Skuric" + given-names: "Antun" + orcid: "https://orcid.org/0000-0002-3323-4482" + - family-names: "Bank" + given-names: "Hasan Sinan" + orcid: "https://orcid.org/0000-0002-0626-2664" + - family-names: "Unger" + given-names: "Richard" + - family-names: "Williams" + given-names: "Owen" + - family-names: "González-Reyes" + given-names: "David" + orcid: "https://orcid.org/0000-0002-1535-3007" + doi: "10.21105/joss.04232" + journal: "Journal of Open Source Software" + url: "https://doi.org/10.21105/joss.04232" + month: 6 + start: 4232 # First page number + end: 4232 # Last page number + title: "SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors" + volume: 7 + issue: 74 + year: 2022 diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 00000000..88763e95 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,128 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, religion, or sexual identity +and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +* Demonstrating empathy and kindness toward other people +* Being respectful of differing opinions, viewpoints, and experiences +* Giving and gracefully accepting constructive feedback +* Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +* Focusing on what is best not just for us as individuals, but for the + overall community + +Examples of unacceptable behavior include: + +* The use of sexualized language or imagery, and sexual attention or + advances of any kind +* Trolling, insulting or derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or email + address, without their explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official e-mail address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +info@simplefoc.com. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series +of actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or +permanent ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within +the community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.0, available at +https://www.contributor-covenant.org/version/2/0/code_of_conduct.html. + +Community Impact Guidelines were inspired by [Mozilla's code of conduct +enforcement ladder](https://github.com/mozilla/diversity). + +[homepage]: https://www.contributor-covenant.org + +For answers to common questions about this code of conduct, see the FAQ at +https://www.contributor-covenant.org/faq. Translations are available at +https://www.contributor-covenant.org/translations. diff --git a/README.md b/README.md index c0e019d6..4fbb7517 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,23 @@ -# Arduino Simple Field Oriented Control (FOC) library - - -![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) +# SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
+### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO + +[![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://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) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) + We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. @@ -11,39 +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: - - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) - - -
-

NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

- -
- -## Arduino *SimpleFOClibrary* v2.1 +- 🎯 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

@@ -53,58 +59,43 @@ Therefore this is an attempt to: This video demonstrates the *Simple**FOC**library* basic usage, electronic connections and shows its capabilities. - ### Features -- **Arduino compatible**: - - Arduino library code - - Arduino Library Manager integration +- **Easy install**: + - Arduino IDE: Arduino Library Manager integration + - PlatformIO - **Open-Source**: Full code and documentation available on github +- **Goal**: + - Support as many [sensor](https://docs.simplefoc.com/position_sensors) + [motor](https://docs.simplefoc.com/motors) + [driver](https://docs.simplefoc.com/drivers) + [current sense](https://docs.simplefoc.com/current_sense) combination as possible. + - Provide the up-to-date and in-depth documentation with API references and the examples - **Easy to setup and configure**: - - Easy hardware configuration - - Easy [tuning the control loops](https://docs.simplefoc.com/motion_control) -- **Modular**: - - Supports as many [sensors, BLDC motors and driver boards](https://docs.simplefoc.com/supported_hardware) as possible - - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers): - - Arduino: UNO, MEGA, any board with ATMega328 chips - - STM32 boards: [Nucleo](https://www.st.com/en/evaluation-tools/stm32-nucleo-boards.html), [Bluepill](https://stm32-base.org/boards/STM32F103C8T6-Blue-Pill.html) ... - - ESP32 - - Teensy boards -- **Plug & play**: Arduino SimpleFOCShield + - Easy hardware configuration + - Each hardware component is a C++ object (easy to understand) + - Easy [tuning the control loops](https://docs.simplefoc.com/motion_control) + - [*Simple**FOC**Studio*](https://docs.simplefoc.com/studio) configuration GUI tool + - Built-in communication and monitoring +- **Cross-platform**: + - Seamless code transfer from one microcontroller family to another + - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers): + - Arduino: UNO R4, UNO, MEGA, DUE, Leonardo, Nano, Nano33 .... + - STM32 + - ESP32 + - Teensy + - many more ...

-## Arduino *SimpleFOCShield* v2.0.3 -

- - - -

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

+## Documentation +Full API code documentation as well as example projects and step by step guides can be found on our [docs website](https://docs.simplefoc.com/). +![image](https://user-images.githubusercontent.com/36178713/168475410-105e4e3d-082a-4015-98ff-d380c7992dfd.png) ## Getting Started Depending on if you want to use this library as the plug and play Arduino library or you want to get insight in the algorithm and make changes there are two ways to install this code. - Full library installation [Docs](https://docs.simplefoc.com/library_download) -- Minimal project builder [Docs](https://docs.simplefoc.com/minimal_download) +- PlatformIO [Docs](https://docs.simplefoc.com/library_platformio) ### Arduino *SimpleFOClibrary* installation to Arduino IDE #### Arduino Library Manager @@ -127,14 +118,21 @@ git clone https://github.com/simplefoc/Arduino-FOC.git ``` - Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`. -### *SimpleFOClibrary* minimal project builder +## Community and contributing -For those willing to experiment and to modify the code I suggest using the minimal project builder [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal). - > This code is completely independent and you can run it as any other Arduino Sketch without the need for any libraries. +For all the questions regarding the potential implementation, applications, supported hardware and similar please visit our [community forum](https://community.simplefoc.com) or our [discord server](https://discord.gg/kWBwuzY32n). -All you need to do is: -- Go to [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal) -- Follow the tutorial in the README file and choose only the library files that are necessary for your application. +It is always helpful to hear the stories/problems/suggestions of people implementing the code and you might find a lot of answered questions there already! + +### Github Issues & Pull requests + +Please do not hesitate to leave an issue if you have problems/advices/suggestions regarding the code! + +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 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. @@ -208,13 +206,30 @@ Here are some of the *Simple**FOC**library* and *Simple**FOC**Shield* applicatio

-## Documentation -Find out more information about the Arduino SimpleFOC project in [docs website](https://docs.simplefoc.com/) +## Citing the *SimpleFOC* +We are very happy that *Simple**FOC**library* has been used as a component of several research project and has made its way to several scientific papers. We are hoping that this trend is going to continue as the project matures and becomes more robust! +A short resume paper about *Simple**FOC*** has been published in the Journal of Open Source Software: +

+ SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors.
+ A. Skuric, HS. Bank, R. Unger, O. Williams, D. González-Reyes
+Journal of Open Source Software, 7(74), 4232, https://doi.org/10.21105/joss.04232 +

-## Arduino FOC repo structure -Branch | Description | Status ------------- | ------------- | ------------ -[master](https://github.com/simplefoc/Arduino-FOC) | Stable and tested library version | ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) -[dev](https://github.com/simplefoc/Arduino-FOC/tree/dev) | Development library version | ![Library Dev Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Dev%20Compile/badge.svg?branch=dev) -[minimal](https://github.com/simplefoc/Arduino-FOC/tree/minimal) | Minimal Arduino example with integrated library | ![MinimalBuild](https://github.com/simplefoc/Arduino-FOC/workflows/MinimalBuild/badge.svg?branch=minimal) +If you are interested in citing *Simple**FOC**library* or some other component of *Simple**FOC**project* in your research, we suggest you to cite our paper: + +```bib +@article{simplefoc2022, + doi = {10.21105/joss.04232}, + url = {https://doi.org/10.21105/joss.04232}, + year = {2022}, + publisher = {The Open Journal}, + volume = {7}, + number = {74}, + pages = {4232}, + author = {Antun Skuric and Hasan Sinan Bank and Richard Unger and Owen Williams and David González-Reyes}, + title = {SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors}, + journal = {Journal of Open Source Software} +} + +``` diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 83451aa7..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 @@ -6,12 +6,13 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); -BLDCDriver6PWM driver = BLDCDriver6PWM(PHASE_UH, PHASE_UL, PHASE_VH, PHASE_VL, PHASE_WH, PHASE_WL); -LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT); +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); // encoder instance -Encoder encoder = Encoder(HALL2, HALL3, 2048, HALL1); +Encoder encoder = Encoder(A_HALL2, A_HALL3, 2048, A_HALL1); // Interrupt routine intialisation // channel A and B callbacks @@ -19,14 +20,18 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void doIndex(){encoder.handleIndex();} -// angle set point variable -float target_angle = 0; // instantiate the commander Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } +void 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); @@ -40,6 +45,8 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); + // link current sense and the driver + currentSense.linkDriver(&driver); // current sensing currentSense.init(); @@ -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); @@ -94,14 +98,12 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function + motor.loopFOC(); // Motion control function - motor.move(target_angle); + motor.move(); // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! @@ -109,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 6c75855a..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); @@ -106,10 +109,8 @@ void setup() { motor.target = 2; // define the motor id - command.add('A', onMotor, "motor"); + 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.")); diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 202aa337..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); @@ -79,6 +85,10 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage motor.voltage_sensor_align = 0.5; // control loop type and torque mode @@ -111,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 @@ -124,7 +130,6 @@ void setup() { motor.init(); cs.init(); - cs.driverSync(&driver); // driver 8302 has inverted gains on all channels cs.gain_a *=-1; cs.gain_b *=-1; diff --git a/examples/hardware_specific_examples/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 new file mode 100644 index 00000000..e1b4c392 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -0,0 +1,169 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A PA8 +#define INH_B PA9 +#define INH_C PA10 + +#define EN_GATE PB7 +#define M_PWM PB4 +#define M_OC PB3 +#define OC_ADJ PB6 +#define OC_GAIN PB5 + +#define IOUTA PA0 +#define IOUTB PA1 +#define IOUTC PA2 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(PB14, PB15, 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 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 19; + driver.pwm_frequency = 15000; // suggested under 18khz + 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/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 new file mode 100644 index 00000000..504ab9c8 --- /dev/null +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -0,0 +1,138 @@ +/* + Odrive robotics' hardware is one of the best BLDC motor foc supporting hardware out there. + + This is an example code that can be directly uploaded to the Odrive using the SWD programmer. + This code uses an encoder with 500 cpr and a BLDC motor with 7 pole pairs connected to the M0 interface of the Odrive. + + This is a short template code and the idea is that you are able to adapt to your needs not to be a complete solution. :D +*/ +#include + +// Odrive M0 motor pinout +#define M0_INH_A PA8 +#define M0_INH_B PA9 +#define M0_INH_C PA10 +#define M0_INL_A PB13 +#define M0_INL_B PB14 +#define M0_INL_C PB15 +// M0 currnets +#define M0_IB PC0 +#define M0_IC PC1 +// Odrive M0 encoder pinout +#define M0_ENC_A PB4 +#define M0_ENC_B PB5 +#define M0_ENC_Z PC9 + + +// Odrive M1 motor pinout +#define M1_INH_A PC6 +#define M1_INH_B PC7 +#define M1_INH_C PC8 +#define M1_INL_A PA7 +#define M1_INL_B PB0 +#define M1_INL_C PB1 +// M0 currnets +#define M1_IB PC2 +#define M1_IC PC3 +// Odrive M1 encoder pinout +#define M1_ENC_A PB6 +#define M1_ENC_B PB7 +#define M1_ENC_Z PC15 + +// M1 & M2 common enable pin +#define EN_GATE PB12 + +// SPI pinout +#define SPI3_SCL PC10 +#define SPI3_MISO PC11 +#define SPI3_MOSO PC12 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver6PWM driver = BLDCDriver6PWM(M0_INH_A,M0_INL_A, M0_INH_B,M0_INL_B, M0_INH_C,M0_INL_C, EN_GATE); + +// instantiate the commander +Commander command = Commander(Serial); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +// low side current sensing define +// 0.0005 Ohm resistor +// gain of 10x +// current sensing on B and C phases, phase A not connected +LowsideCurrentSense current_sense = LowsideCurrentSense(0.0005f, 10.0f, _NC, M0_IB, M0_IC); + +Encoder encoder = Encoder(M0_ENC_A, M0_ENC_B, 500,M0_ENC_Z); +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} +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] + driver.voltage_power_supply = 20; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 20; + // driver init + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB, doI); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + + // max voltage allowed for motion control + motor.voltage_limit = 8.0; + // alignment voltage limit + motor.voltage_sensor_align = 0.5; + + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; + motor.monitor_downsample = 1000; + + // add target command T + command.add('M', doMotor, "motor M0"); + + // initialise motor + motor.init(); + + // link the driver + current_sense.linkDriver(&driver); + // init the current sense + current_sense.init(); + current_sense.skip_align = true; + motor.linkCurrentSense(¤t_sense); + + // init FOC + motor.initFOC(); + delay(1000); +} + +void loop(){ + + // foc loop + motor.loopFOC(); + // motion control + motor.move(); + // monitoring + motor.monitor(); + // user communication + command.run(); +} \ No newline at end of file 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 new file mode 100644 index 00000000..7abcde54 --- /dev/null +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -0,0 +1,136 @@ +/* + Odrive robotics' hardware is one of the best BLDC motor foc supporting hardware out there. + + This is an example code that can be directly uploaded to the Odrive using the SWD programmer. + This code uses an magnetic spi sensor AS5047 and a BLDC motor with 11 pole pairs connected to the M0 interface of the Odrive. + + This is a short template code and the idea is that you are able to adapt to your needs not to be a complete solution. :D +*/ +#include + +// Odrive M0 motor pinout +#define M0_INH_A PA8 +#define M0_INH_B PA9 +#define M0_INH_C PA10 +#define M0_INL_A PB13 +#define M0_INL_B PB14 +#define M0_INL_C PB15 +// M0 currnets +#define M0_IB PC0 +#define M0_IC PC1 +// Odrive M0 encoder pinout +#define M0_ENC_A PB4 +#define M0_ENC_B PB5 +#define M0_ENC_Z PC9 + + +// Odrive M1 motor pinout +#define M1_INH_A PC6 +#define M1_INH_B PC7 +#define M1_INH_C PC8 +#define M1_INL_A PA7 +#define M1_INL_B PB0 +#define M1_INL_C PB1 +// M0 currnets +#define M1_IB PC2 +#define M1_IC PC3 +// Odrive M1 encoder pinout +#define M1_ENC_A PB6 +#define M1_ENC_B PB7 +#define M1_ENC_Z PC15 + +// M1 & M2 common enable pin +#define EN_GATE PB12 + +// SPI pinout +#define SPI3_SCL PC10 +#define SPI3_MISO PC11 +#define SPI3_MOSO PC12 + +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver6PWM driver = BLDCDriver6PWM(M0_INH_A,M0_INL_A, M0_INH_B,M0_INL_B, M0_INH_C,M0_INL_C, EN_GATE); + +// instantiate the commander +Commander command = Commander(Serial); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +// low side current sensing define +// 0.0005 Ohm resistor +// gain of 10x +// current sensing on B and C phases, phase A not connected +LowsideCurrentSense current_sense = LowsideCurrentSense(0.0005f, 10.0f, _NC, M0_IB, M0_IC); + +// MagneticSensorSPI(int cs, float _cpr, int _angle_register) +// config - SPI config +// cs - SPI chip select pin +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, M0_ENC_A); +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] + driver.voltage_power_supply = 20; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 20; + // driver init + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // initialise magnetic sensor hardware + sensor.init(&SPI_3); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + + // max voltage allowed for motion control + motor.voltage_limit = 8.0; + // alignment voltage limit + motor.voltage_sensor_align = 0.5; + + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; + motor.monitor_downsample = 1000; + + // add target command T + command.add('M', doMotor, "motor M0"); + + // initialise motor + motor.init(); + + // link the driver + current_sense.linkDriver(&driver); + // init the current sense + current_sense.init(); + current_sense.skip_align = true; + motor.linkCurrentSense(¤t_sense); + + // init FOC + motor.initFOC(); + delay(1000); +} + +void loop(){ + + // foc loop + motor.loopFOC(); + // motion control + motor.move(); + // monitoring + motor.monitor(); + // user communication + command.run(); +} \ No newline at end of file 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 dfad6e3a..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 @@ -12,7 +12,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} // inline current sensor instance -InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); +InlineCurrentSense current_sense = InlineCurrentSense(0.001f, 50.0f, A0, A1); // commander communication instance Commander command = Commander(Serial); @@ -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); @@ -33,6 +39,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); motor.voltage_sensor_align = 1; // set control loop type to be used @@ -54,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 new file mode 100644 index 00000000..8eb122a0 --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -0,0 +1,128 @@ +/** + * + * SimpleFOCMini motor control example + * + * For Arduino UNO, the most convenient way to use the board is to stack it to the pins: + * - 12 - GND + * - 11 - IN1 + * - 10 - IN2 + * - 9 - IN3 + * - 8 - EN + * + * For other boards with UNO headers but more PWM channles such as esp32, nucleo-64, samd51 metro etc, the best way to most convenient pinout is: + * - GND - GND + * - 13 - IN1 + * - 12 - IN2 + * - 11 - IN3 + * - 9 - EN + * + * For the boards without arduino uno headers, the choice of pinout is a lot less constrained. + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// 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); + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::angle; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle P controller + motor.P_angle.P = 20; + // maximal velocity of the position control + motor.velocity_limit = 4; + + // 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 angle using serial terminal:")); + _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/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 259b23ee..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 @@ -24,11 +24,11 @@ void doB2(){encoder2.handleB();} // inline current sensor instance // check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) -InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2); +InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, A0, A2); // inline current sensor instance // check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) -InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); +InlineCurrentSense current_sense2 = InlineCurrentSense(0.01f, 50.0f, A1, A3); // commander communication instance Commander command = Commander(Serial); @@ -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); @@ -56,11 +62,16 @@ void setup() { driver1.init(); // link driver motor1.linkDriver(&driver1); + // link current sense and the driver + current_sense1.linkDriver(&driver1); + // power supply voltage [V] driver2.voltage_power_supply = 12; driver2.init(); // link driver motor2.linkDriver(&driver2); + // link current sense and the driver + current_sense2.linkDriver(&driver2); // set control loop type to be used motor1.controller = MotionControlType::torque; @@ -83,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 2d6c3ca6..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 @@ -13,15 +13,21 @@ void doB(){encoder.handleB();} // inline current sensor instance // check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor) -InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +void doMotion(char* cmd){ command.motion(&motor, cmd); } // void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // 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); @@ -34,6 +40,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // set control loop type to be used motor.controller = MotionControlType::torque; @@ -53,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 @@ -74,7 +79,7 @@ void setup() { motor.target = 2; // subscribe motor to the commander - command.add('T', doTarget, "target"); + command.add('T', doMotion, "motion control"); // command.add('M', doMotor, "motor"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) diff --git a/examples/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 new file mode 100644 index 00000000..6359f201 --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -0,0 +1,104 @@ + +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(6, 10, 5, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// inline current sensor instance +// ACS712-05B has the resolution of 0.185mV per Amp +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); + +// commander communication instance +Commander command = Commander(Serial); +void doMotion(char* cmd){ command.motion(&motor, cmd); } +// 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); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); + + // set control loop type to be used + motor.controller = MotionControlType::torque; + + // controller configuration based on the control type + motor.PID_velocity.P = 0.05f; + motor.PID_velocity.I = 1; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 20; + + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_downsample = 0; // disable intially + motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle + + // current sense init and linking + current_sense.init(); + motor.linkCurrentSense(¤t_sense); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + // subscribe motor to the commander + command.add('T', doMotion, "motion control"); + // command.add('M', doMotor, "motor"); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor ready."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // motor monitoring + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/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/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino new file mode 100644 index 00000000..7f59551d --- /dev/null +++ b/examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -0,0 +1,41 @@ +// 6pwm standalone example code for Teensy 3.x boards +#include + + +// BLDC driver instance +// using FTM0 timer +BLDCDriver6PWM driver = BLDCDriver6PWM(22,23, 9,10, 6,20, 8); +// using FTM3 timer - available on Teensy3.5 and Teensy3.6 +// BLDCDriver6PWM driver = BLDCDriver6PWM(2,14, 7,8, 35,36, 8); + +void setup() { + Serial.begin(115200); + // Enable debugging + // Driver init will show debugging output + SimpleFOCDebug::enable(&Serial); + + // pwm frequency to be used [Hz] + driver.pwm_frequency = 30000; + // dead zone percentage of the duty cycle - default 0.02 - 2% + driver.dead_zone=0.02; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + // phase C: 5V + driver.setPwm(3,6,5); +} \ No newline at end of file diff --git a/examples/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 new file mode 100644 index 00000000..9143dde7 --- /dev/null +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -0,0 +1,70 @@ +// 6pwm openloop velocity example +#include + + +// BLDC motor & driver instance +// BLDCMotor motor = BLDCMotor(pole pair number); +BLDCMotor motor = BLDCMotor(11); +// using FTM0 timer +// BLDCDriver6PWM(pwmA_H, pwmA_L, pwmB_H,pwmB_L, pwmC_H, pwmC_L) +BLDCDriver6PWM driver = BLDCDriver6PWM(22,23, 9,10, 6,20, 8); +// using FTM3 timer - available on Teensy3.5 and Teensy3.6 +// BLDCDriver6PWM driver = BLDCDriver6PWM(2,14, 7,8, 35,36, 8); + +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&motor.target, cmd); } +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; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp + motor.voltage_limit = 3; // [V] + + // open loop control config + motor.controller = MotionControlType::velocity_openloop; + + // init motor hardware + motor.init(); + + //initial motor target + motor.target=0; + + // add target command T + command.add('T', doTarget, "target velocity"); + command.add('L', doLimit, "voltage limit"); + + Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); + _delay(1000); +} + +void loop() { + + // open loop velocity movement + // using motor.voltage_limit + motor.move(); + + // user communication + command.run(); +} diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino new file mode 100644 index 00000000..f3df3db1 --- /dev/null +++ b/examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -0,0 +1,57 @@ +// 6pwm standalone example code for Teensy 4.x boards +// +// Teensy4.x 6pwm driver generates a 6pwm signal using FlexTimers and it doesn't support the QuadTimers +// - Each high-low pair of the 6pwm has to be A and B channels of the same FlexTimer and to the same submodule +// +// List of available teensy 4.1 pins with their respective submodules and channels +// FlexPWM(timer number)_(submodule)_(channel) +// FlexPWM4_2_A pin 2 +// FlexPWM4_2_B pin 3 +// FlexPWM1_3_B pin 7 +// FlexPWM1_3_A pin 8 +// FlexPWM2_2_A pin 6 +// FlexPWM2_2_B pin 9 +// FlexPWM3_1_B pin 28 +// FlexPWM3_1_A pin 29 +// FlexPWM2_3_A pin 36 +// FlexPWM2_3_B pin 37 +#include + + +// BLDC driver instance +// make sure to provide channel A for high side and channel B for low side +// BLDCDriver6PWM(pwmA_H, pwmA_L, pwmB_H,pwmB_L, pwmC_H, pwmC_L) +// Example configuration +BLDCDriver6PWM driver = BLDCDriver6PWM(2,3, 6,9, 8,7); + +void setup() { + Serial.begin(115200); + // Enable debugging + // Driver init will show debugging output + SimpleFOCDebug::enable(&Serial); + + // pwm frequency to be used [Hz] + driver.pwm_frequency = 30000; + // dead zone percentage of the duty cycle - default 0.02 - 2% + driver.dead_zone=0.02; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + // phase C: 5V + driver.setPwm(3,6,5); +} \ No newline at end of file diff --git a/examples/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 new file mode 100644 index 00000000..5ff53311 --- /dev/null +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -0,0 +1,85 @@ +// 6pwm openloop velocity example +// +// Teensy4.x 6pwm driver generates a 6pwm signal using FlexTimers and it doesn't support the QuadTimers +// - Each high-low pair of the 6pwm has to be A and B channels of the same FlexTimer and to the same submodule +// +// List of available teensy 4.1 pins with their respective submodules and channels +// FlexPWM(timer number)_(submodule)_(channel) +// FlexPWM4_2_A pin 2 +// FlexPWM4_2_B pin 3 +// FlexPWM1_3_B pin 7 +// FlexPWM1_3_A pin 8 +// FlexPWM2_2_A pin 6 +// FlexPWM2_2_B pin 9 +// FlexPWM3_1_B pin 28 +// FlexPWM3_1_A pin 29 +// FlexPWM2_3_A pin 36 +// FlexPWM2_3_B pin 37 +#include + + +// BLDC motor & driver instance +// BLDCMotor motor = BLDCMotor(pole pair number); +BLDCMotor motor = BLDCMotor(11); +// make sure to provide channel A for high side and channel B for low side +// BLDCDriver6PWM(pwmA_H, pwmA_L, pwmB_H,pwmB_L, pwmC_H, pwmC_L) +// Example configuration +BLDCDriver6PWM driver = BLDCDriver6PWM(2,3, 6,9, 8,7); + +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&motor.target, cmd); } +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; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp + motor.voltage_limit = 3; // [V] + + // open loop control config + motor.controller = MotionControlType::velocity_openloop; + + // init motor hardware + motor.init(); + + //initial motor target + motor.target=0; + + // add target command T + command.add('T', doTarget, "target velocity"); + command.add('L', doLimit, "voltage limit"); + + Serial.println("Motor ready!"); + Serial.println("Set target velocity [rad/s]"); + _delay(1000); +} + +void loop() { + + // open loop velocity movement + // using motor.voltage_limit + motor.move(); + + // user communication + command.run(); +} diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index a4756352..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 @@ -18,29 +18,53 @@ float target_position = 0; // instantiate the commander Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_position, cmd); } +void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } +void 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; - driver.init(); + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp motor.voltage_limit = 3; // [V] + // limit/set the velocity of the transition in between + // target angles motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::angle_openloop; // 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); @@ -49,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 f7a82eb3..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 @@ -19,30 +19,49 @@ float target_velocity = 0; // instantiate the commander Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } +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; - driver.init(); + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // current = voltage / resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] - motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::velocity_openloop; // 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); @@ -52,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 942f73dc..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 @@ -24,9 +24,6 @@ * */ #include -// software interrupt library -#include -#include // BLDC motor & driver instance BLDCMotor motor = BLDCMotor(11); @@ -36,15 +33,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); // encoder instance -Encoder encoder = Encoder(2, 3, 8192, A0); +Encoder encoder = Encoder(2, 3, 8192); // Interrupt routine intialisation // channel A and B callbacks void doA(){encoder.handleA();} void doB(){encoder.handleB();} -void doIndex(){encoder.handleIndex();} -// If no available hadware interrupt pins use the software interrupt -PciListenerImp listenerIndex(encoder.index_pin, doIndex); // angle set point variable float target_angle = 0; @@ -54,11 +48,15 @@ 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); - // software interrupts - PciManager.registerListener(&listenerIndex); // link the motor to the sensor motor.linkSensor(&encoder); @@ -71,8 +69,6 @@ void setup() { // aligning voltage [V] motor.voltage_sensor_align = 3; - // index search velocity [rad/s] - motor.velocity_index_search = 3; // set motion control loop to be used motor.controller = MotionControlType::angle; @@ -98,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 8b1ec96b..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 @@ -17,7 +17,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} // current sensor -InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); // current set point variable float target_current = 0; @@ -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); @@ -39,6 +45,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // current sense init hardware current_sense.init(); @@ -68,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 a57fe634..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); @@ -77,7 +81,7 @@ void setup() { motor.initFOC(); // add target command T - command.add('T', doTarget, "target voltage"); + command.add('T', doTarget, "target velocity"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); @@ -103,4 +107,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 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 3fcd7b30..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 @@ -35,9 +35,10 @@ void testAlignmentAndCogging(int direction) { for (int i = 0; i < sample_count; i++) { - float electricAngle = (float) direction * i * motor.pole_pairs * shaft_rotation / sample_count; + float shaftAngle = (float) direction * i * shaft_rotation / sample_count; + float electricAngle = (float) shaftAngle * motor.pole_pairs; // move and wait - motor.move(electricAngle * PI / 180); + motor.move(shaftAngle * PI / 180); _delay(5); // measure @@ -79,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 new file mode 100644 index 00000000..c39657b1 --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,106 @@ +/** + * + * Find KV rating for motor with encoder + * + * Motor KV rating is defiend as the increase of the motor velocity expressed in rotations per minute [rpm] per each 1 Volt int voltage control mode. + * + * This example will set your motor in the torque control mode using voltage and set 1 volt to the motor. By reading the velocity it will calculat the motors KV rating. + * - To make this esimation more credible you can try increasing the target voltage (or decrease in some cases) + * - The KV rating should be realatively static number - it should not change considerably with the increase in the voltage + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// encoder instance +Encoder sensor = Encoder(2, 3, 8192); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} + + +// voltage set point variable +float target_voltage = 1; + +// instantiate the commander +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/_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); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // IMPORTANT! + // make sure to set the correct power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + command.add('K', calcKV, "calculate KV rating"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage : - commnad T")); + Serial.println(F("Calculate the motor KV : - command K")); + _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(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file 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 new file mode 100644 index 00000000..4eadd376 --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,103 @@ +/** + * + * Find KV rating for motor with Hall sensors + * + * Motor KV rating is defiend as the increase of the motor velocity expressed in rotations per minute [rpm] per each 1 Volt int voltage control mode. + * + * This example will set your motor in the torque control mode using voltage and set 1 volt to the motor. By reading the velocity it will calculat the motors KV rating. + * - To make this esimation more credible you can try increasing the target voltage (or decrease in some cases) + * - The KV rating should be realatively static number - it should not change considerably with the increase in the voltage + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// hall sensor instance +HallSensor sensor = HallSensor(2, 3, 4, 11); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} + + +// voltage set point variable +float target_voltage = 1; + +// instantiate the commander +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/_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); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // IMPORTANT! + // make sure to set the correct power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + command.add('K', calcKV, "calculate KV rating"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage : - commnad T")); + Serial.println(F("Calculate the motor KV : - command K")); + _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(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file 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 new file mode 100644 index 00000000..1df631af --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,100 @@ +/** + * Find KV rating for motor with magnetic sensors + * + * Motor KV rating is defiend as the increase of the motor velocity expressed in rotations per minute [rpm] per each 1 Volt int voltage control mode. + * + * This example will set your motor in the torque control mode using voltage and set 1 volt to the motor. By reading the velocity it will calculat the motors KV rating. + * - To make this esimation more credible you can try increasing the target voltage (or decrease in some cases) + * - The KV rating should be realatively static number - it should not change considerably with the increase in the voltage + */ +#include + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +// magnetic sensor instance - I2C +// MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); +// magnetic sensor instance - analog output +// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// voltage set point variable +float target_voltage = 1; + +// instantiate the commander +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/_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 + motor.linkSensor(&sensor); + + // driver config + // IMPORTANT! + // make sure to set the correct power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + command.add('K', calcKV, "calculate KV rating"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage : - commnad T")); + Serial.println(F("Calculate the motor KV : - command K")); + _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(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index c5dfbf43..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,15 +54,13 @@ void setup() { // initialize motor motor.init(); - // monitoring port - Serial.begin(115200); // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); Serial.println("-\n"); float pp_search_voltage = 4; // maximum power_supply_voltage/2 - float pp_search_angle = 6*M_PI; // search electrical angle to turn + float pp_search_angle = 6*_PI; // search electrical angle to turn // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; @@ -73,6 +77,7 @@ void setup() { while(motor_angle <= pp_search_angle){ motor_angle += 0.01f; motor.move(motor_angle); + _delay(1); } _delay(1000); // read the encoder value for 180 @@ -89,9 +94,9 @@ void setup() { Serial.print(F("Estimated PP : ")); Serial.println(pp); Serial.println(F("PP = Electrical angle / Encoder angle ")); - Serial.print(pp_search_angle*180/M_PI); + Serial.print(pp_search_angle*180/_PI); Serial.print("/"); - Serial.print((angle_end-angle_begin)*180/M_PI); + Serial.print((angle_end-angle_begin)*180/_PI); Serial.print(" = "); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index 0b07c772..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,15 +53,12 @@ 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"); float pp_search_voltage = 4; // maximum power_supply_voltage/2 - float pp_search_angle = 6*M_PI; // search electrical angle to turn + float pp_search_angle = 6*_PI; // search electrical angle to turn // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; @@ -73,6 +76,7 @@ void setup() { motor_angle += 0.01f; sensor.update(); // keep track of the overflow motor.move(motor_angle); + _delay(1); } _delay(1000); // read the sensor value for 180 @@ -89,9 +93,9 @@ void setup() { Serial.print(F("Estimated PP : ")); Serial.println(pp); Serial.println(F("PP = Electrical angle / Encoder angle ")); - Serial.print(pp_search_angle*180/M_PI); + Serial.print(pp_search_angle*180/_PI); Serial.print(F("/")); - Serial.print((angle_end-angle_begin)*180/M_PI); + Serial.print((angle_end-angle_begin)*180/_PI); Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); 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 new file mode 100644 index 00000000..2448a51c --- /dev/null +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -0,0 +1,66 @@ +/** + * An example code for the generic current sensing implementation +*/ +#include + + +// user defined function for reading the phase currents +// returning the value per phase in amps +PhaseCurrent_s readCurrentSense(){ + PhaseCurrent_s c; + // dummy example only reading analog pins + c.a = analogRead(A0); + c.b = analogRead(A1); + c.c = analogRead(A2); // if no 3rd current sense set it to 0 + return(c); +} + +// user defined function for intialising the current sense +// it is optional and if provided it will be called in current_sense.init() +void initCurrentSense(){ + pinMode(A0,INPUT); + pinMode(A1,INPUT); + pinMode(A2,INPUT); +} + + +// GenericCurrentSense class constructor +// it receives the user defined callback for reading the current sense +// and optionally the user defined callback for current sense initialisation +GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense); + + +void setup() { + + // 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; + //current_sense.initCallback = initCurrentSense; + + // initialise the current sensing + current_sense.init(); + + + Serial.println("Current sense ready."); +} + +void loop() { + + PhaseCurrent_s currents = current_sense.getPhaseCurrents(); + float current_magnitude = current_sense.getDCCurrent(); + + Serial.print(currents.a); // milli Amps + Serial.print("\t"); + Serial.print(currents.b); // milli Amps + Serial.print("\t"); + Serial.print(currents.c); // milli Amps + Serial.print("\t"); + Serial.println(current_magnitude); // milli Amps +} \ No newline at end of file diff --git a/examples/utils/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 798273c1..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 @@ -7,17 +7,26 @@ // shunt resistor value // gain value // pins phase A,B, (C optional) -InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); +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/generic_sensor/generic_sensor.ino b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino new file mode 100644 index 00000000..4a470e59 --- /dev/null +++ b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino @@ -0,0 +1,51 @@ +/** + * Generic sensor example code + * + * This is a code intended to demonstrate how to implement the generic sensor class + * + */ + +#include + +// sensor reading function example +// for the magnetic sensor with analog communication +// returning an angle in radians in between 0 and 2PI +float readSensor(){ + return analogRead(A0)*_2PI/1024.0; +} + +// sensor intialising function +void initSensor(){ + pinMode(A0,INPUT); +} + +// generic sensor class contructor +// - read sensor callback +// - init sensor callback (optional) +GenericSensor sensor = GenericSensor(readSensor, initSensor); + +void setup() { + // monitoring port + Serial.begin(115200); + + // if callbacks are not provided in the constructor + // they can be assigned directly: + //sensor.readCallback = readSensor; + //sensor.initCallback = initSensor; + + sensor.init(); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} \ No newline at end of file diff --git a/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 ef2e504c..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 @@ -11,14 +11,7 @@ // 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, 11); - -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){sensor.handleA();} -void doB(){sensor.handleB();} -void doC(){sensor.handleC();} - +HallSensor sensor = HallSensor(2, 3, 4, 14); void setup() { // monitoring port @@ -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); @@ -44,4 +35,5 @@ void loop() { Serial.print(sensor.getAngle()); Serial.print("\t"); Serial.println(sensor.getVelocity()); + delay(100); } 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/examples/utils/sensor_test/linear_hall_sensors/find_raw_centers/find_raw_centers.ino b/examples/utils/sensor_test/linear_hall_sensors/find_raw_centers/find_raw_centers.ino new file mode 100644 index 00000000..5fda5f59 --- /dev/null +++ b/examples/utils/sensor_test/linear_hall_sensors/find_raw_centers/find_raw_centers.ino @@ -0,0 +1,62 @@ +/** + * An example to find the center offsets for both ADC channels used in the LinearHall sensor constructor + * Spin your motor through at least one full revolution to average out all of the variations in magnet strength. + */ + +//Change these defines to match the analog input pins that your hall sensors are connected to +#define LINEAR_HALL_CHANNEL_A 39 +#define LINEAR_HALL_CHANNEL_B 33 + + +//program variables +int minA, maxA, minB, maxB, centerA, centerB; +unsigned long timestamp; + +void setup() { + // monitoring port + Serial.begin(115200); + + // initialise magnetic sensor hardware + pinMode(LINEAR_HALL_CHANNEL_A, INPUT); + pinMode(LINEAR_HALL_CHANNEL_B, INPUT); + + minA = analogRead(LINEAR_HALL_CHANNEL_A); + maxA = minA; + centerA = (minA + maxA) / 2; + minB = analogRead(LINEAR_HALL_CHANNEL_B); + maxB = minB; + centerB = (minB + maxB) / 2; + + Serial.println("Sensor ready"); + delay(1000); + timestamp = millis(); +} + +void loop() { + //read sensors and update variables + int tempA = analogRead(LINEAR_HALL_CHANNEL_A); + if (tempA < minA) minA = tempA; + if (tempA > maxA) maxA = tempA; + centerA = (minA + maxA) / 2; + int tempB = analogRead(LINEAR_HALL_CHANNEL_B); + if (tempB < minB) minB = tempB; + if (tempB > maxB) maxB = tempB; + centerB = (minB + maxB) / 2; + + if (millis() > timestamp + 100) { + timestamp = millis(); + // display the center counts, and max and min count + Serial.print("A:"); + Serial.print(centerA); + Serial.print("\t, B:"); + Serial.print(centerB); + Serial.print("\t, min A:"); + Serial.print(minA); + Serial.print("\t, max A:"); + Serial.print(maxA); + Serial.print("\t, min B:"); + Serial.print(minB); + Serial.print("\t, max B:"); + Serial.println(maxB); + } +} diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index c930437b..0516ede1 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -18,7 +18,7 @@ void setup() { // Normally SimpleFOC will call begin for i2c but with esp32 begin() is the only way to set pins! // It seems safe to call begin multiple times - Wire1.begin(19, 23, 400000); + Wire1.begin(19, 23, (uint32_t)400000); sensor0.init(); sensor1.init(&Wire1); diff --git a/keywords.txt b/keywords.txt index b47c18fd..d8fd3e73 100644 --- a/keywords.txt +++ b/keywords.txt @@ -22,7 +22,9 @@ LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 - +GenericCurrentSense KEYWORD1 +GenericSensor KEYWORD1 +SimpleFOCDebug KEYWORD1 initFOC KEYWORD2 loopFOC KEYWORD2 @@ -89,7 +91,7 @@ getFOCCurrents KEYWORD2 getDCCurrent KEYWORD2 setPwm KEYWORD2 driverAlign KEYWORD2 -driverSync KEYWORD2 +linkDriver KEYWORD2 add KEYWORD2 run KEYWORD2 attach KEYWORD2 @@ -102,6 +104,8 @@ lpf KEYWORD2 motor KEYWORD2 handlePWM KEYWORD2 enableInterrupt KEYWORD2 +readCallback KEYWORD2 +initCallback KEYWORD2 @@ -118,6 +122,7 @@ pullup KEYWORD2 quadrature KEYWORD2 foc_modulation KEYWORD2 target KEYWORD2 +motion KEYWORD2 pwm_frequency KEYWORD2 dead_zone KEYWORD2 gain_a KEYWORD2 @@ -128,8 +133,10 @@ sensor_direction KEYWORD2 sensor_offset KEYWORD2 zero_electric_angle KEYWORD2 verbose KEYWORD2 +verboseMode KEYWORD1 decimal_places KEYWORD2 phase_resistance KEYWORD2 +phase_inductance KEYWORD2 modulation_centered KEYWORD2 @@ -163,6 +170,10 @@ voltage_power_supply KEYWORD2 voltage_sensor_align KEYWORD2 velocity_index_search KEYWORD2 monitor_downsample KEYWORD2 +monitor_start_char KEYWORD2 +monitor_end_char KEYWORD2 +monitor_separator KEYWORD2 +monitor_decimals KEYWORD2 monitor_variables KEYWORD2 motion_downsample KEYWORD2 @@ -230,6 +241,7 @@ _PI_3 LITERAL1 _SQRT3 LITERAL1 _PI LITERAL1 _HIGH_Z LITERAL1 +_NC LITERAL1 _MON_TARGET LITERAL1 _MON_VOLT_Q LITERAL1 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 89540636..5daa49d9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.2 +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 ff451e07..501a8bc9 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,15 +1,55 @@ #include "BLDCMotor.h" +#include "./communication/SimpleFOCDebug.h" + + +// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 +// each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z +int trap_120_map[6][3] = { + {_HIGH_IMPEDANCE,1,-1}, + {-1,1,_HIGH_IMPEDANCE}, + {-1,_HIGH_IMPEDANCE,1}, + {_HIGH_IMPEDANCE,-1,1}, + {1,-1,_HIGH_IMPEDANCE}, + {1,_HIGH_IMPEDANCE,-1} +}; + +// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 +// each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z +int trap_150_map[12][3] = { + {_HIGH_IMPEDANCE,1,-1}, + {-1,1,-1}, + {-1,1,_HIGH_IMPEDANCE}, + {-1,1,1}, + {-1,_HIGH_IMPEDANCE,1}, + {-1,-1,1}, + {_HIGH_IMPEDANCE,-1,1}, + {1,-1,1}, + {1,-1,_HIGH_IMPEDANCE}, + {1,-1,-1}, + {1,_HIGH_IMPEDANCE,-1}, + {1,1,-1} +}; // BLDCMotor( int pp , float R) // - pp - pole pair number // - R - motor phase resistance -BLDCMotor::BLDCMotor(int pp, float _R) +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance +BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) : FOCMotor() { // save pole pairs number pole_pairs = pp; // save phase resistance number phase_resistance = _R; + // save back emf constant KV = 1/KV + // 1/sqrt(2) - rms value + KV_rating = NOT_SET; + if (_isset(_KV)) + KV_rating = _KV; + // save phase inductance + phase_inductance = _inductance; + // torque control type is voltage by default torque_controller = TorqueControlType::voltage; } @@ -23,15 +63,15 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { } // init hardware pins -void BLDCMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); - - // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit - if( !current_sense && _isset(phase_resistance)) { - float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case - // use it if it is less then voltage_limit set by the user - voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; +int BLDCMotor::init() { + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return 0; } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + // sanity check for the voltage limit configuration if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment @@ -42,26 +82,38 @@ void BLDCMotor::init() { // current control loop controls voltage PID_current_q.limit = voltage_limit; PID_current_d.limit = voltage_limit; + } + if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){ // velocity control loop controls current PID_velocity.limit = current_limit; - }else if(!current_sense && _isset(phase_resistance)){ - PID_velocity.limit = current_limit; }else{ + // velocity control loop controls the voltage PID_velocity.limit = voltage_limit; } P_angle.limit = velocity_limit; + // 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 - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + 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 @@ -76,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; } @@ -84,40 +143,53 @@ 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 if(monitor_port) monitor_port->println(F("MOT: No sensor.")); - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - _delay(500); - if(exit_flag){ - if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); + // 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){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; disable(); } @@ -128,18 +200,17 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction int BLDCMotor::alignCurrentSense() { int exit_flag = 1; // success - if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); + SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection - if(monitor_port) monitor_port->println(F("MOT: Align error!")); + SIMPLEFOC_DEBUG("MOT: Align error!"); exit_flag = 0; }else{ // output the alignment status flag - if(monitor_port) monitor_port->print(F("MOT: Success: ")); - if(monitor_port) monitor_port->println(exit_flag); + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); } return exit_flag > 0; @@ -148,20 +219,26 @@ int BLDCMotor::alignCurrentSense() { // Encoder alignment to electrical 0 angle int BLDCMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // check if sensor needs zero search + if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); + // 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)){ - // check if sensor needs zero search - if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); - // stop init if not found index - if(!exit_flag) return exit_flag; + 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); } // take and angle in the middle @@ -170,39 +247,41 @@ 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 - if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + float moved = fabs(mid_angle - end_angle); + if (movedprintln(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); - float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + 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 { + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + } else { SIMPLEFOC_DEBUG("MOT: Skip dir calib."); } // zero electric angle not known if(!_isset(zero_electric_angle)){ // 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(); @@ -212,13 +291,12 @@ int BLDCMotor::alignSensor() { //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -227,7 +305,7 @@ int BLDCMotor::alignSensor() { int BLDCMotor::absoluteZeroSearch() { // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision // of float is sufficient. - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -247,8 +325,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) { SIMPLEFOC_DEBUG("MOT: Error: Not found!"); } + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -282,7 +360,9 @@ void BLDCMotor::loopFOC() { current.q = LPF_current_q(current.q); // calculate the phase voltage voltage.q = PID_current_q(current_sp - current.q); - voltage.d = 0; + // 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; @@ -294,10 +374,12 @@ void BLDCMotor::loopFOC() { // 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 - if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); break; } @@ -312,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; @@ -324,20 +409,27 @@ void BLDCMotor::move(float new_target) { // when switching to a 2-component representation. if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode - // get angular velocity + // get angular velocity TODO the velocity reading probably also shouldn't happen in open loop modes? shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + 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; + // upgrade the current based voltage limit switch (controller) { case MotionControlType::torque: if(torque_controller == TorqueControlType::voltage){ // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; - else voltage.q = target*phase_resistance; - voltage.d = 0; + 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 } @@ -349,15 +441,18 @@ 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 if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*phase_resistance; - voltage.d = 0; + 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: @@ -369,8 +464,10 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*phase_resistance; - voltage.d = 0; + 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: @@ -396,7 +493,7 @@ void BLDCMotor::move(float new_target) { // Function implementing Space Vector PWM and Sine PWM algorithms // // Function using sine approximation -// regular sin + cos ~300us (no memory usaage) +// regular sin + cos ~300us (no memory usage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { @@ -408,13 +505,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { { case FOCModulationType::Trapezoid_120 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 - static int trap_120_map[6][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_120_state = 0; + // determine the sector sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == true > driver.voltage_limit/2 // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = modulation_centered ? (driver->voltage_limit)/2 : Uq; @@ -422,30 +516,27 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ua= center; Ub = trap_120_map[sector][1] * Uq + center; Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible + driver->setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // disable phase if possible }else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){ Ua = trap_120_map[sector][0] * Uq + center; Ub = center; Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible + driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON);// disable phase if possible }else{ Ua = trap_120_map[sector][0] * Uq + center; Ub = trap_120_map[sector][1] * Uq + center; Uc = center; - driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible + driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF);// disable phase if possible } break; case FOCModulationType::Trapezoid_150 : // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 - static int trap_150_map[12][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_150_state = 0; + // determine the sector sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 + // modulation_centered == true > driver.voltage_limit/2 // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 center = modulation_centered ? (driver->voltage_limit)/2 : Uq; @@ -453,135 +544,64 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { Ua= center; Ub = trap_150_map[sector][1] * Uq + center; Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible + driver->setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // disable phase if possible }else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){ Ua = trap_150_map[sector][0] * Uq + center; Ub = center; Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ + driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON); // disable phase if possible + }else if(trap_150_map[sector][2] == _HIGH_IMPEDANCE){ Ua = trap_150_map[sector][0] * Uq + center; Ub = trap_150_map[sector][1] * Uq + center; Uc = center; - driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible + driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF); // disable phase if possible + }else{ + Ua = trap_150_map[sector][0] * Uq + center; + Ub = trap_150_map[sector][1] * Uq + center; + Uc = trap_150_map[sector][2] * Uq + center; + driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // enable all phases } 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 @@ -608,8 +628,11 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; - + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -643,10 +666,13 @@ float BLDCMotor::angleOpenloop(float target_angle){ shaft_velocity = 0; } - // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle // sensor precision: this calculation is OK due to the normalisation setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index e5947590..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" @@ -18,16 +19,18 @@ class BLDCMotor: public FOCMotor /** BLDCMotor class constructor @param pp pole pairs number - @param R motor phase resistance + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] */ - BLDCMotor(int pp, float R = NOT_SET); + BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); /** * Function linking a motor and a foc driver * * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting */ - void linkDriver(BLDCDriver* driver); + virtual void linkDriver(BLDCDriver* driver); /** * BLDCDriver link: @@ -37,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 */ @@ -47,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 @@ -67,12 +70,8 @@ class BLDCMotor: public FOCMotor void move(float target = NOT_SET) override; float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - - - private: - // FOC methods - /** + + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm * @@ -80,7 +79,11 @@ class BLDCMotor: public FOCMotor * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float Ud, float angle_el); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + + private: + // FOC methods + /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Current sense and motor phase alignment */ diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4f9f1108..4e6815e5 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -45,7 +45,7 @@ // BLDCMotor( pole_pairs ) BLDCMotor motor = BLDCMotor(11); // BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) -BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // Encoder(pin_A, pin_B, CPR) Encoder encoder = Encoder(2, 3, 2048); // channel A and B callbacks @@ -104,13 +104,16 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" +#include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" #include "current_sense/LowsideCurrentSense.h" +#include "current_sense/GenericCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" +#include "communication/SimpleFOCDebug.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2acd6f11..1d8f3147 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,15 +1,24 @@ #include "StepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + // StepperMotor(int pp) // - pp - pole pair number // - R - motor phase resistance -StepperMotor::StepperMotor(int pp, float _R) +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance) : FOCMotor() { // number od pole pairs pole_pairs = pp; // save phase resistance number phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV; + // save phase inductance + phase_inductance = _inductance; // torque control type is voltage by default // current and foc_current not supported yet @@ -24,15 +33,15 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { } // init hardware pins -void StepperMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); - - // if set the phase resistance of the motor use current limit to calculate the voltage limit - if(_isset(phase_resistance)) { - float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case - // use it if it is less then voltage_limit set by the user - voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; +int StepperMotor::init() { + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return 0; } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + // sanity check for the voltage limit configuration if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment @@ -47,12 +56,21 @@ 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 - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); + motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } @@ -82,44 +100,90 @@ 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 if(monitor_port) monitor_port->println(F("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){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; disable(); } 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 - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + 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 @@ -129,7 +193,8 @@ 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); } // take and angle in the middle @@ -138,7 +203,8 @@ 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); } sensor->update(); @@ -147,30 +213,33 @@ int StepperMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + 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 { + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + } else { + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } // zero electric angle not known if(!_isset(zero_electric_angle)){ // 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(); @@ -179,13 +248,12 @@ int StepperMotor::alignSensor() { zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -193,7 +261,7 @@ int StepperMotor::alignSensor() { // - to the index int StepperMotor::absoluteZeroSearch() { - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -213,8 +281,8 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -230,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; @@ -240,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); } @@ -252,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; @@ -270,47 +372,73 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target) ) target = new_target; - // choose control loop + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + 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; + + // 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.d = 0; + 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 = current_sp*phase_resistance; - voltage.d = 0; + 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 = current_sp*phase_resistance; - voltage.d = 0; + 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; 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; @@ -329,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; @@ -361,7 +486,11 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -395,7 +524,11 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); @@ -403,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 d0cd3e18..7e7810d8 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -23,9 +23,11 @@ class StepperMotor: public FOCMotor /** StepperMotor class constructor @param pp pole pair number - @param R motor phase resistance + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] */ - StepperMotor(int pp, float R = NOT_SET); + StepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); /** * Function linking a motor and a foc driver @@ -41,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 */ @@ -52,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 @@ -75,12 +73,7 @@ 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 - - private: - - // FOC methods - /** + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm * @@ -88,12 +81,16 @@ class StepperMotor: public FOCMotor * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float Ud , float angle_el); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + private: + /** Sensor alignment to electrical 0 angle of the motor */ 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 a657d17c..6ae8186f 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -2,28 +2,17 @@ #define BLDCDRIVER_H #include "Arduino.h" +#include "FOCDriver.h" -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 /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -38,7 +27,10 @@ class BLDCDriver{ * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) */ - virtual void setPhaseState(int sa, int sb, int sc) = 0; + 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 2cec8dd5..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,47 +8,74 @@ 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; - }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; + }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; + }else 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); @@ -57,11 +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; -} \ No newline at end of file +} + +/** + Driver linking to the current sense +*/ +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 0904dea2..d3f7f8ed 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,40 +1,74 @@ #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: /** * Function intialising the CurrentSense class - * All the necessary intialisations of adc and sync should be implemented here + * - All the necessary intialisations of adc and sync should be implemented here + * + * @returns - 0 - for failure & 1 - for success */ - virtual void init() = 0; + virtual int init() = 0; /** - * Function intended to implement all that is needed to sync and current sensing with the driver. - * If no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & 1 - for success + * Linking the current sense with the motor driver + * Only necessary if synchronisation in between the two is required */ - virtual int driverSync(BLDCDriver *driver) = 0; + void linkDriver(FOCDriver *driver); - // calibration variables + // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() + + 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(BLDCDriver *driver, float voltage) = 0; + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -47,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) * @@ -56,12 +90,56 @@ 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); + }; #endif \ No newline at end of file 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 922076a6..5d8f8127 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -1,4 +1,5 @@ #include "FOCMotor.h" +#include "../../communication/SimpleFOCDebug.h" /** * Default constructor - setting all variabels to default values @@ -28,10 +29,18 @@ FOCMotor::FOCMotor() current_sp = 0; current.q = 0; current.d = 0; + + // 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; //sensor + sensor_offset = 0.0f; sensor = nullptr; //current sensor current_sense = nullptr; @@ -77,65 +86,74 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print - if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); + #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; if(monitor_variables & _MON_TARGET){ - monitor_port->print(target,4); - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + monitor_port->print(target,monitor_decimals); printed= true; } if(monitor_variables & _MON_VOLT_Q) { - monitor_port->print(voltage.q,4); - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(voltage.q,monitor_decimals); printed= true; } if(monitor_variables & _MON_VOLT_D) { - monitor_port->print(voltage.d,4); - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(voltage.d,monitor_decimals); printed= true; } // read currents if possible - even in voltage mode (if current_sense available) if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) { - DQCurrent_s c{0,0}; - if(current_sense){ - if(torque_controller == TorqueControlType::foc_current) c = current; - else{ - c = current_sense->getFOCCurrents(electrical_angle); - c.q = LPF_current_q(c.q); - c.d = LPF_current_d(c.d); - } + DQCurrent_s c = current; + if( current_sense && torque_controller != TorqueControlType::foc_current ){ + c = current_sense->getFOCCurrents(electrical_angle); + c.q = LPF_current_q(c.q); + c.d = LPF_current_d(c.d); } if(monitor_variables & _MON_CURR_Q) { - monitor_port->print(c.q*1000, 2); // mAmps - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(c.q*1000, monitor_decimals); // mAmps printed= true; } if(monitor_variables & _MON_CURR_D) { - monitor_port->print(c.d*1000, 2); // mAmps - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(c.d*1000, monitor_decimals); // mAmps printed= true; } } if(monitor_variables & _MON_VEL) { - monitor_port->print(shaft_velocity,4); - monitor_port->print("\t"); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(shaft_velocity,monitor_decimals); printed= true; } if(monitor_variables & _MON_ANGLE) { - monitor_port->print(shaft_angle,4); + if(!printed && monitor_start_char) monitor_port->print(monitor_start_char); + else if(printed) monitor_port->print(monitor_separator); + monitor_port->print(shaft_angle,monitor_decimals); printed= true; } - if(printed) monitor_port->println(); - -} + if(printed){ + if(monitor_end_char) monitor_port->println(monitor_end_char); + else monitor_port->println(""); + } +} diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 37c5b260..8ae0e8af 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -24,33 +24,48 @@ /** * Motiron control type */ -enum MotionControlType{ - torque,//!< Torque control - velocity,//!< Velocity motion control - angle,//!< Position/angle motion control - velocity_openloop, - angle_openloop +enum MotionControlType : uint8_t { + torque = 0x00, //!< Torque control + velocity = 0x01, //!< Velocity motion control + angle = 0x02, //!< Position/angle motion control + velocity_openloop = 0x03, + angle_openloop = 0x04 }; /** * Motiron control type */ -enum TorqueControlType{ - voltage, //!< Torque control using voltage - dc_current, //!< Torque control using DC current (one current magnitude) - foc_current //!< torque control using dq currents +enum TorqueControlType : uint8_t { + voltage = 0x00, //!< Torque control using voltage + dc_current = 0x01, //!< Torque control using DC current (one current magnitude) + foc_current = 0x02, //!< torque control using dq currents }; /** * FOC modulation type */ -enum FOCModulationType{ - SinePWM, //!< Sinusoidal PWM modulation - SpaceVectorPWM, //!< Space vector modulation method - Trapezoid_120, - Trapezoid_150 +enum FOCModulationType : uint8_t { + SinePWM = 0x00, //!< Sinusoidal PWM modulation + SpaceVectorPWM = 0x01, //!< Space vector modulation method + Trapezoid_120 = 0x02, + Trapezoid_150 = 0x03, }; + + +enum FOCMotorStatus : uint8_t { + motor_uninitialized = 0x00, //!< Motor is not yet initialized + motor_initializing = 0x01, //!< Motor intiialization is in progress + motor_uncalibrated = 0x02, //!< Motor is initialized, but not calibrated (open loop possible) + motor_calibrating = 0x03, //!< Motor calibration in progress + motor_ready = 0x04, //!< Motor is initialized and calibrated (closed loop possible) + motor_error = 0x08, //!< Motor is in error state (recoverable, e.g. overcurrent protection active) + motor_calib_failed = 0x0E, //!< Motor calibration failed (possibly recoverable) + motor_init_failed = 0x0F, //!< Motor initialization failed (not recoverable) +}; + + + /** Generic motor class */ @@ -63,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 */ @@ -89,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 @@ -112,6 +123,16 @@ class FOCMotor */ virtual void move(float target = NOT_SET)=0; + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0; + // State calculation methods /** Shaft angle calculation in radians [rad] */ float shaftAngle(); @@ -122,6 +143,7 @@ class FOCMotor float shaftVelocity(); + /** * Electrical angle calculation */ @@ -129,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 @@ -137,6 +160,9 @@ class FOCMotor float shaft_angle_sp;//!< current target angle DQVoltage_s voltage;//!< current d and q voltage set to the motor DQCurrent_s current;//!< current d and q current 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 @@ -145,17 +171,20 @@ class FOCMotor // motor physical parameters float phase_resistance; //!< motor phase resistance int pole_pairs;//!< motor pole pairs number + float KV_rating; //!< motor KV rating + float phase_inductance; //!< motor phase inductance // limiting variables - float voltage_limit; //!< Voltage limitting variable - global limit - float current_limit; //!< Current limitting variable - global limit - float velocity_limit; //!< Velocity limitting variable - global limit + float voltage_limit; //!< Voltage limiting variable - global limit + float current_limit; //!< Current limiting variable - global limit + float velocity_limit; //!< Velocity limiting variable - global limit // motor status vairables int8_t enabled = 0;//!< enabled or disabled motor flag + FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status // pwm modulation related variables - FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm + FOCModulationType foc_modulation;//!< parameter determining modulation algorithm int8_t modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2 or (0) pulled to 0 @@ -178,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 @@ -194,6 +224,10 @@ class FOCMotor */ void monitor(); unsigned int monitor_downsample = DEF_MON_DOWNSMAPLE; //!< show monitor outputs each monitor_downsample calls + char monitor_start_char = '\0'; //!< monitor starting character + char monitor_end_char = '\0'; //!< monitor outputs ending character + char monitor_separator = '\t'; //!< monitor outputs separation character + unsigned int monitor_decimals = 4; //!< monitor outputs decimal places // initial monitoring will display target, voltage, velocity and angle uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 23a51d9b..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,10 +2,12 @@ #include "../foc_utils.h" #include "../time_utils.h" -// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 + 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,18 +19,24 @@ 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; - // quick fix for strange cases (micros overflow) - if(Ts <= 0) Ts = 1e-3f; - // velocity calculation - float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; - // save variables for future pass + 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; vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; vel_angle_prev_ts = angle_prev_ts; - return vel; + return velocity; } + + void Sensor::init() { // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero) getSensorAngle(); // call once diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index efab1be1..c77eb911 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -6,19 +6,19 @@ /** * Direction structure */ -enum Direction{ - CW = 1, //clockwise +enum Direction : int8_t { + CW = 1, // clockwise CCW = -1, // counter clockwise - UNKNOWN = 0 //not yet known or invalid state + UNKNOWN = 0 // not yet known or invalid state }; /** * Pullup configuration structure */ -enum Pullup{ - USE_INTERN, //!< Use internal pullups - USE_EXTERN //!< Use external pullups +enum Pullup : uint8_t { + USE_INTERN = 0x00, //!< Use internal pullups + USE_EXTERN = 0x01 //!< Use external pullups }; /** @@ -42,6 +42,7 @@ enum Pullup{ * */ 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 @@ -101,6 +102,12 @@ class Sensor{ * 1 - ecoder with index (with index not found yet) */ virtual int needsSearch(); + + /** + * Minimum time between updates to velocity. If time elapsed is lower than this, the velocity is not updated. + */ + float min_elapsed_time = 0.000100; // default is 100 microseconds, or 10kHz + protected: /** * Get current shaft angle from the sensor hardware, and @@ -120,9 +127,10 @@ class Sensor{ virtual void init(); // velocity calculation variables - float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity + float velocity=0.0f; + float angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity - float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity + float vel_angle_prev=0.0f; // angle at last call to getVelocity, used for velocity long vel_angle_prev_ts=0; // last velocity calculation timestamp int32_t full_rotations=0; // full rotation tracking int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 33f08843..9864b235 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,27 +1,30 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H -class StepperDriver{ +#include "Arduino.h" +#include "FOCDriver.h" + +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 - /** - * 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 df651e2a..ac57daa1 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -28,7 +28,7 @@ #define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample @@ -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 e0358750..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -5,11 +5,16 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#ifndef _round #define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) +#endif #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) #define _UNUSED(v) (void) (v) +#define _powtwo(x) (1 << (x)) + +#define _swap(a, b) { auto temp = a; a = b; b = temp; } // utility defines #define _2_SQRT3 1.15470053838f @@ -24,11 +29,15 @@ #define _2PI 6.28318530718f #define _3PI_2 4.71238898038f #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 ((int) NOT_SET) + +#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f) // dq current structure struct DQCurrent_s @@ -49,6 +58,12 @@ struct DQVoltage_s float d; float q; }; +// alpha beta current structure +struct ABCurrent_s +{ + float alpha; + float beta; +}; /** @@ -65,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/common/pid.cpp b/src/common/pid.cpp index aeeccf1a..da1bee13 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -56,3 +56,9 @@ float PIDController::operator() (float error){ timestamp_prev = timestamp_now; return output; } + +void PIDController::reset(){ + integral_prev = 0.0f; + output_prev = 0.0f; + error_prev = 0.0f; +} diff --git a/src/common/pid.h b/src/common/pid.h index 203d3c82..acd68d6f 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -23,6 +23,7 @@ class PIDController ~PIDController() = default; float operator() (float error); + void reset(); float P; //!< Proportional gain float I; //!< Integral gain diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 8e1cb051..d2822b04 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -1,6 +1,5 @@ #include "Commander.h" - Commander::Commander(Stream& serial, char eol, bool echo){ com_port = &serial; this->eol = eol; @@ -12,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++; } @@ -63,6 +62,7 @@ void Commander::run(char* user_input){ switch(id){ case CMD_SCAN: for(int i=0; i < call_count; i++){ + printMachineReadable(CMD_SCAN); print(call_ids[i]); print(":"); if(call_label[i]) println(call_label[i]); @@ -72,6 +72,7 @@ void Commander::run(char* user_input){ case CMD_VERBOSE: if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]); printVerbose(F("Verb:")); + printMachineReadable(CMD_VERBOSE); switch (verbose){ case VerboseMode::nothing: println(F("off!")); @@ -80,16 +81,21 @@ void Commander::run(char* user_input){ case VerboseMode::user_friendly: println(F("on!")); break; + case VerboseMode::machine_readable: + printlnMachineReadable(F("machine")); + break; } break; case CMD_DECIMAL: if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]); printVerbose(F("Decimal:")); + printMachineReadable(CMD_DECIMAL); println(decimal_places); break; default: for(int i=0; i < call_count; i++){ if(id == call_ids[i]){ + printMachineReadable(user_input[0]); call_list[i](&user_input[1]); break; } @@ -101,10 +107,8 @@ void Commander::run(char* user_input){ void Commander::motor(FOCMotor* motor, char* user_command) { // if target setting - if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){ - printVerbose(F("Target: ")); - motor->target = atof(user_command); - println(motor->target); + if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+' || isSentinel(user_command[0])){ + target(motor, user_command); return; } @@ -112,12 +116,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) { char cmd = user_command[0]; char sub_cmd = user_command[1]; // check if there is a subcommand or not - int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1; + int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') || (sub_cmd == '#') ? 2 : 1; // check if get command bool GET = isSentinel(user_command[value_index]); // parse command values float value = atof(&user_command[value_index]); - + printMachineReadable(cmd); + if (sub_cmd >= 'A' && sub_cmd <= 'Z') { + printMachineReadable(sub_cmd); + } // a bit of optimisation of variable memory for Arduino UNO (atmega328) switch(cmd){ @@ -178,59 +185,9 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_MOTION_TYPE: - printVerbose(F("Motion:")); - switch(sub_cmd){ - case SCMD_DOWNSAMPLE: - printVerbose(F(" downsample: ")); - if(!GET) motor->motion_downsample = value; - println((int)motor->motion_downsample); - break; - default: - // change control type - if(!GET && value >= 0 && (int)value < 5) // if set command - motor->controller = (MotionControlType)value; - switch(motor->controller){ - case MotionControlType::torque: - println(F("torque")); - break; - case MotionControlType::velocity: - println(F("vel")); - break; - case MotionControlType::angle: - println(F("angle")); - break; - case MotionControlType::velocity_openloop: - println(F("vel open")); - break; - case MotionControlType::angle_openloop: - println(F("angle open")); - break; - } - break; - } - break; case CMD_TORQUE_TYPE: - // change control type - printVerbose(F("Torque: ")); - if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command - motor->torque_controller = (TorqueControlType)value; - switch(motor->torque_controller){ - case TorqueControlType::voltage: - println(F("volt")); - break; - case TorqueControlType::dc_current: - println(F("dc curr")); - break; - case TorqueControlType::foc_current: - println(F("foc curr")); - break; - } - break; case CMD_STATUS: - // enable/disable - printVerbose(F("Status: ")); - if(!GET) (bool)value ? motor->enable() : motor->disable(); - println(motor->enabled); + motion(motor, &user_command[0]); break; case CMD_PWMMOD: // PWM modulation change @@ -265,18 +222,31 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } break; case CMD_RESIST: - // enable/disable printVerbose(F("R phase: ")); if(!GET){ motor->phase_resistance = value; - if(motor->torque_controller==TorqueControlType::voltage){ - motor->voltage_limit = motor->current_limit*value; + if(motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit= motor->current_limit; - } } if(_isset(motor->phase_resistance)) println(motor->phase_resistance); else println(0); break; + case CMD_INDUCTANCE: + printVerbose(F("L phase: ")); + if(!GET){ + motor->phase_inductance = value; + } + if(_isset(motor->phase_inductance)) println(motor->phase_inductance); + else println(0); + break; + case CMD_KV_RATING: + printVerbose(F("Motor KV: ")); + if(!GET){ + motor->KV_rating = value; + } + if(_isset(motor->KV_rating)) println(motor->KV_rating); + else println(0); + break; case CMD_SENSOR: // Sensor zero offset printVerbose(F("Sensor | ")); @@ -359,12 +329,23 @@ void Commander::motor(FOCMotor* motor, char* user_command) { motor->monitor_variables = (uint8_t) 0; println(F("clear")); break; + case CMD_DECIMAL: + printVerbose(F("decimal: ")); + motor->monitor_decimals = value; + println((int)motor->monitor_decimals); + break; case SCMD_SET: - if(!GET) motor->monitor_variables = (uint8_t) 0; + if(!GET){ + // set the variables + motor->monitor_variables = (uint8_t) 0; + for(int i = 0; i < 7; i++){ + if(isSentinel(user_command[value_index+i])) break; + motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); + } + } + // print the variables for(int i = 0; i < 7; i++){ - if(isSentinel(user_command[value_index+i])) break; - if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i); - print( (user_command[value_index+i] - '0') ); + print( (motor->monitor_variables & (1 << (6-i))) >> (6-i)); } println(""); break; @@ -379,6 +360,80 @@ void Commander::motor(FOCMotor* motor, char* user_command) { } } +void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ + char cmd = user_cmd[0]; + char sub_cmd = user_cmd[1]; + bool GET = isSentinel(user_cmd[1]); + float value = atof(&user_cmd[(sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1]); + + switch(cmd){ + case CMD_MOTION_TYPE: + printVerbose(F("Motion:")); + switch(sub_cmd){ + case SCMD_DOWNSAMPLE: + printVerbose(F(" downsample: ")); + if(!GET) motor->motion_downsample = value; + println((int)motor->motion_downsample); + break; + default: + // change control type + if(!GET && value >= 0 && (int)value < 5) // if set command + motor->controller = (MotionControlType)value; + switch(motor->controller){ + case MotionControlType::torque: + println(F("torque")); + break; + case MotionControlType::velocity: + println(F("vel")); + break; + case MotionControlType::angle: + println(F("angle")); + break; + case MotionControlType::velocity_openloop: + println(F("vel open")); + break; + case MotionControlType::angle_openloop: + println(F("angle open")); + break; + } + break; + } + break; + case CMD_TORQUE_TYPE: + // change control type + printVerbose(F("Torque: ")); + if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command + motor->torque_controller = (TorqueControlType)value; + switch(motor->torque_controller){ + case TorqueControlType::voltage: + println(F("volt")); + // change the velocity control limits if necessary + if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit; + break; + case TorqueControlType::dc_current: + println(F("dc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; + break; + case TorqueControlType::foc_current: + println(F("foc curr")); + // change the velocity control limits if necessary + motor->PID_velocity.limit = motor->current_limit; + break; + } + break; + case CMD_STATUS: + // enable/disable + printVerbose(F("Status: ")); + if(!GET) (bool)value ? motor->enable() : motor->disable(); + println(motor->enabled); + break; + default: + target(motor, user_cmd, separator); + break; + } +} + void Commander::pid(PIDController* pid, char* user_cmd){ char cmd = user_cmd[0]; bool GET = isSentinel(user_cmd[1]); @@ -439,6 +494,98 @@ void Commander::scalar(float* value, char* user_cmd){ println(*value); } + +void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){ + // if no values sent + if(isSentinel(user_cmd[0])) { + printlnMachineReadable(motor->target); + return; + }; + + float pos, vel, torque; + char* next_value; + switch(motor->controller){ + case MotionControlType::torque: // setting torque target + torque = atof(strtok (user_cmd, separator)); + motor->target = torque; + break; + case MotionControlType::velocity: // setting velocity target + torque limit + // set the target + vel= atof(strtok (user_cmd, separator)); + motor->target = vel; + + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value){ + torque = atof(next_value); + motor->PID_velocity.limit = torque; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + break; + case MotionControlType::angle: // setting angle target + torque, velocity limit + // setting the target position + pos= atof(strtok (user_cmd, separator)); + motor->target = pos; + + // allow for setting only the target position without chaning the velocity/torque limits + next_value = strtok (NULL, separator); + if( next_value ){ + vel = atof(next_value); + motor->velocity_limit = vel; + motor->P_angle.limit = vel; + + // allow for setting only the target position and velocity limit without the torque limit + next_value = strtok (NULL, separator); + if( next_value ){ + torque= atof(next_value); + motor->PID_velocity.limit = torque; + // torque command can be voltage or current + if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + } + break; + case MotionControlType::velocity_openloop: // setting velocity target + torque limit + // set the target + vel= atof(strtok (user_cmd, separator)); + motor->target = vel; + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value ){ + torque = atof(next_value); + // torque command can be voltage or current + if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + break; + case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit + // set the target + pos= atof(strtok (user_cmd, separator)); + motor->target = pos; + + // allow for setting only the target position without chaning the velocity/torque limits + next_value = strtok (NULL, separator); + if( next_value ){ + vel = atof(next_value); + motor->velocity_limit = vel; + // allow for setting only the target velocity without chaning the torque limit + next_value = strtok (NULL, separator); + if (next_value ){ + torque = atof(next_value); + // torque command can be voltage or current + if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque; + else motor->current_limit = torque; + } + } + break; + } + printVerbose(F("Target: ")); + println(motor->target); +} + + bool Commander::isSentinel(char ch) { if(ch == eol) @@ -446,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; } @@ -499,6 +647,39 @@ void Commander::printVerbose(const char* message){ void Commander::printVerbose(const __FlashStringHelper *message){ if(verbose == VerboseMode::user_friendly) print(message); } + +void Commander::printMachineReadable(const int number){ + if(verbose == VerboseMode::machine_readable) print(number); +} +void Commander::printMachineReadable(const float number){ + if(verbose == VerboseMode::machine_readable) print(number); +} +void Commander::printMachineReadable(const char* message){ + if(verbose == VerboseMode::machine_readable) print(message); +} +void Commander::printMachineReadable(const __FlashStringHelper *message){ + if(verbose == VerboseMode::machine_readable) print(message); +} +void Commander::printMachineReadable(const char message){ + if(verbose == VerboseMode::machine_readable) print(message); +} + +void Commander::printlnMachineReadable(const int number){ + if(verbose == VerboseMode::machine_readable) println(number); +} +void Commander::printlnMachineReadable(const float number){ + if(verbose == VerboseMode::machine_readable) println(number); +} +void Commander::printlnMachineReadable(const char* message){ + if(verbose == VerboseMode::machine_readable) println(message); +} +void Commander::printlnMachineReadable(const __FlashStringHelper *message){ + if(verbose == VerboseMode::machine_readable) println(message); +} +void Commander::printlnMachineReadable(const char message){ + if(verbose == VerboseMode::machine_readable) println(message); +} + void Commander::printError(){ println(F("err")); } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 13777867..4ec2b281 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -12,10 +12,11 @@ // Commander verbose display to the user type -enum VerboseMode{ - nothing = 0, // display nothing - good for monitoring - on_request, // display only on user request - user_friendly // display textual messages to the user +enum VerboseMode : uint8_t { + nothing = 0x00, // display nothing - good for monitoring + on_request = 0x01, // display only on user request + user_friendly = 0x02, // display textual messages to the user + machine_readable = 0x03 // display machine readable commands, matching commands to set each settings }; @@ -90,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 @@ -100,9 +101,13 @@ class Commander Stream* com_port = nullptr; //!< Serial terminal variable if provided char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) + /** * * FOC motor (StepperMotor and BLDCMotor) command interface + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) * 'D' - D current PID controller & LPF (see function pid and lpf for commands) @@ -139,7 +144,11 @@ class Commander * 'C' - clear monitor * 'S' - set monitoring variables * 'G' - get variable value - * '' - Target get/set + * '' - Target setting interface + * Depends of the motion control mode: + * - torque : torque (ex. M2.5) + * - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits) + * - angle (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits) * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) @@ -149,6 +158,9 @@ class Commander /** * Low pass fileter command interface + * @param lpf - LowPassFilter instance + * @param user_cmd - the string command + * * - It only has one property - filtering time constant Tf * - It can be get by sending 'F' * - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01) @@ -156,6 +168,9 @@ class Commander void lpf(LowPassFilter* lpf, char* user_cmd); /** * PID controller command interface + * @param pid - PIDController instance + * @param user_cmd - the string command + * * - It has several paramters (the letters can be changed in the commands.h file) * - P gain - 'P' * - I gain - 'I' @@ -168,12 +183,64 @@ class Commander void pid(PIDController* pid, char* user_cmd); /** * Float variable scalar command interface + * @param value - float variable pointer + * @param user_cmd - the string command + * * - It only has one property - one float value * - It can be get by sending an empty string '\n' * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); + /** + * Target setting interface, enables setting the target and limiting variables at once. + * The values are sent separated by a separator specified as the third argument. The default separator is the space. + * + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * @param separator - the string separator in between target and limit values, default is space - " " + * + * Example: P2.34 70 2 + * `P` is the user defined command, `2.34` is the target angle `70` is the target + * velocity and `2` is the desired max current. + * + * It depends of the motion control mode: + * - torque : torque (ex. P2.5) + * - velocity : velocity torque (ex.P10 2.5 or P10 to only chanage the target witout limits) + * - angle : angle velocity torque (ex.P3.5 10 2.5 or P3.5 to only chanage the target witout limits) + */ + void target(FOCMotor* motor, char* user_cmd, char* separator = (char *)" "); + + /** + * FOC motor (StepperMotor and BLDCMotor) motion control interfaces + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * @param separator - the string separator in between target and limit values, default is space - " " + * + * Commands: + * 'C' - Motion control type config + * sub-commands: + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type + * sub-commands: + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) + * sub-commands: + * '0' - enable + * '1' - disable + * '' - Target setting interface + * Depends of the motion control mode: + * - torque : torque (ex. M2.5) + * - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits) + * - angle (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits) + */ + void 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 @@ -214,8 +281,20 @@ class Commander void println(const __FlashStringHelper *message); void println(const char message); + void printMachineReadable(const float number); + void printMachineReadable(const int number); + void printMachineReadable(const char* message); + void printMachineReadable(const __FlashStringHelper *message); + void printMachineReadable(const char message); + + void printlnMachineReadable(const float number); + void printlnMachineReadable(const int number); + void printlnMachineReadable(const char* message); + void printlnMachineReadable(const __FlashStringHelper *message); + void printlnMachineReadable(const char message); + + void printError(); - bool isSentinel(char ch); }; diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp new file mode 100644 index 00000000..4d50b87c --- /dev/null +++ b/src/communication/SimpleFOCDebug.cpp @@ -0,0 +1,125 @@ + +#include "SimpleFOCDebug.h" + +#ifndef SIMPLEFOC_DISABLE_DEBUG + + +Print* SimpleFOCDebug::_debugPrint = NULL; + + +void SimpleFOCDebug::enable(Print* debugPrint) { + _debugPrint = debugPrint; +} + + +void SimpleFOCDebug::println(int val) { + if (_debugPrint != NULL) { + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(float val) { + if (_debugPrint != NULL) { + _debugPrint->println(val); + } +} + + + +void SimpleFOCDebug::println(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + + +void SimpleFOCDebug::println(const char* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const char* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + + +void SimpleFOCDebug::print(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + + +void SimpleFOCDebug::print(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + +void SimpleFOCDebug::print(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) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::print(float val) { + if (_debugPrint != NULL) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::println() { + if (_debugPrint != NULL) { + _debugPrint->println(); + } +} + +#endif \ No newline at end of file diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h new file mode 100644 index 00000000..d0ff611f --- /dev/null +++ b/src/communication/SimpleFOCDebug.h @@ -0,0 +1,79 @@ + +#ifndef __SIMPLEFOCDEBUG_H__ +#define __SIMPLEFOCDEBUG_H__ + +#include "Arduino.h" + + +/** + * SimpleFOCDebug class + * + * This class is used to print debug messages to a chosen output. + * Currently, Print instances are supported as targets, e.g. serial port. + * + * Activate debug output globally by calling enable(), optionally passing + * in a Print instance. If none is provided "Serial" is used by default. + * + * To produce debug output, use the macro SIMPLEFOC_DEBUG: + * SIMPLEFOC_DEBUG("Debug message!"); + * SIMPLEFOC_DEBUG("a float value:", 123.456f); + * SIMPLEFOC_DEBUG("an integer value: ", 123); + * + * Keep debugging output short and simple. Some of our MCUs have limited + * RAM and limited serial output capabilities. + * + * By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to + * help preserve memory on Arduino boards. + * + * You can also disable debug output completely. In this case all debug output + * and the SimpleFOCDebug class is removed from the compiled code. + * Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in + * this way. + * + **/ + +// #define 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); + +protected: + static Print* _debugPrint; +}; + + +#define SIMPLEFOC_DEBUG(msg, ...) \ + SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) + +#else //ifndef SIMPLEFOC_DISABLE_DEBUG + + + +#define SIMPLEFOC_DEBUG(msg, ...) + + + +#endif //ifndef SIMPLEFOC_DISABLE_DEBUG +#endif + diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index c8f19b47..ee4f69e9 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -13,7 +13,7 @@ void StepDirListener::init(){ } void StepDirListener::enableInterrupt(void (*doA)()){ - attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE); + attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity); } void StepDirListener::attach(float* variable){ @@ -22,15 +22,15 @@ void StepDirListener::attach(float* variable){ void StepDirListener::handle(){ // read step status - bool step = digitalRead(pin_step); + //bool step = digitalRead(pin_step); // update counter only on rising edge - if(step && step != step_active){ - if(digitalRead(pin_dir)) + //if(step && step != step_active){ + if(digitalRead(pin_dir)) count++; - else + else count--; - } - step_active = step; + //} + //step_active = step; // if attached variable update it if(attached_variable) *attached_variable = getValue(); } diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 1c50c700..3627b5e7 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "../common/foc_utils.h" + /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -47,11 +48,13 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing + decltype(RISING) polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable float counter_to_value; //!< step counter to value - bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + }; #endif \ No newline at end of file diff --git a/src/communication/commands.h b/src/communication/commands.h index 3ba6cdde..323a8e7e 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -15,6 +15,8 @@ #define CMD_SENSOR 'S' //!< sensor offsets #define CMD_MONITOR 'M' //!< monitoring #define CMD_RESIST 'R' //!< motor phase resistance + #define CMD_INDUCTANCE 'I' //!< motor phase inductance + #define CMD_KV_RATING 'K' //!< motor kv rating #define CMD_PWMMOD 'W' //!< pwm modulation // commander configuration diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp new file mode 100644 index 00000000..54c4f12e --- /dev/null +++ b/src/current_sense/GenericCurrentSense.cpp @@ -0,0 +1,63 @@ +#include "GenericCurrentSense.h" + +// GenericCurrentSense constructor +GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){ + // if function provided add it to the + if(readCallback != nullptr) this->readCallback = readCallback; + if(initCallback != nullptr) this->initCallback = initCallback; +} + +// Inline sensor init function +int GenericCurrentSense::init(){ + // configure ADC variables + if(initCallback != nullptr) initCallback(); + // calibrate zero offsets + calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); + // return success + return 1; +} +// Function finding zero offsets of the ADC +void GenericCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + PhaseCurrent_s current = readCallback(); + offset_ia += current.a; + offset_ib += current.b; + offset_ic += current.c; + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current = readCallback(); + current.a = (current.a - offset_ia); // amps + current.b = (current.b - offset_ib); // amps + current.c = (current.c - offset_ic); // 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 +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ + _UNUSED(voltage) ; // remove unused parameter warning + int exit_flag = 1; + if(skip_align) return exit_flag; + if (!initialized) return 0; + return exit_flag; +} diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h new file mode 100644 index 00000000..02bf8fa9 --- /dev/null +++ b/src/current_sense/GenericCurrentSense.h @@ -0,0 +1,40 @@ +#ifndef GENERIC_CS_LIB_H +#define GENERIC_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "../common/base_classes/CurrentSense.h" +#include "../common/lowpass_filter.h" +#include "hardware_api.h" + + +class GenericCurrentSense: public CurrentSense{ + public: + /** + GenericCurrentSense class constructor + */ + GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr); + + // CurrentSense interface implementing functions + int init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverAlign(float align_voltage, bool modulation_centered) override; + + + PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading + void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation + + private: + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif \ No newline at end of file diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index bc3bc4c6..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 @@ -17,156 +18,71 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; gain_c = volts_to_amps_ratio; -} +}; + + +InlineCurrentSense::InlineCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +}; + + // Inline sensor init function -void InlineCurrentSense::init(){ +int InlineCurrentSense::init(){ + // if no linked driver its fine in this case + // at least for init() + void* drv_params = driver ? driver->params : nullptr; // configure ADC variables - _configureADCInline(pinA,pinB,pinC); + 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 + return 1; } // Function finding zero offsets of the ADC void InlineCurrentSense::calibrateOffsets(){ const int calibration_rounds = 1000; - + // find adc offset = zero current voltage offset_ia = 0; offset_ib = 0; offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltageInline(pinA); - offset_ib += _readADCVoltageInline(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); + if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params); + if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params); _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / calibration_rounds; - offset_ib = offset_ib / calibration_rounds; + if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds; + if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds; if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps + current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps + current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int InlineCurrentSense::driverSync(BLDCDriver *driver){ - return 1; -} - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// 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(BLDCDriver *driver, float voltage){ - int exit_flag = 1; - if(skip_align) return exit_flag; - - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = fabs(c.a / c.b); - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = fabs(c.b/c.a); - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - - // if phase C measured - if(_isset(pinC)){ - // set phase B active and phases A and C down - driver->setPwm(0, 0, voltage); - _delay(200); - c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 50; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0f; - } - driver->setPwm(0, 0, 0); - gain_c *= _sign(c.c); - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - return exit_flag; -} diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 748d3f44..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: /** @@ -21,32 +24,21 @@ class InlineCurrentSense: public CurrentSense{ @param phC C phase adc pin (optional) */ InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + /** + InlineCurrentSense class constructor + @param mVpA mV per Amp ratio + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + InlineCurrentSense(float mVpA, int pinA, int pinB, int pinC = NOT_SET); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; - - // 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 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 @@ -56,9 +48,6 @@ class InlineCurrentSense: public CurrentSense{ * Function finding zero offsets of the ADC */ void calibrateOffsets(); - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index ca4f51b4..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 @@ -19,18 +20,51 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int gain_c = volts_to_amps_ratio; } + +LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +} + + // Lowside sensor init function -void LowsideCurrentSense::init(){ +int LowsideCurrentSense::init(){ + + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: Driver not linked!"); + return 0; + } + // configure ADC variables - _configureADCLowSide(pinA,pinB,pinC); + params = _configureADCLowSide(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(); + 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 + return 1; } // Function finding zero offsets of the ADC void LowsideCurrentSense::calibrateOffsets(){ - const int calibration_rounds = 1000; + const int calibration_rounds = 2000; // find adc offset = zero current voltage offset_ia = 0; @@ -39,14 +73,14 @@ void LowsideCurrentSense::calibrateOffsets(){ // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { _startADC3PinConversionLowSide(); - offset_ia += _readADCVoltageLowSide(pinA); - offset_ib += _readADCVoltageLowSide(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + if(_isset(pinA)) offset_ia += (_readADCVoltageLowSide(pinA, params)); + if(_isset(pinB)) offset_ib += (_readADCVoltageLowSide(pinB, params)); + if(_isset(pinC)) offset_ic += (_readADCVoltageLowSide(pinC, params)); _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / calibration_rounds; - offset_ib = offset_ib / calibration_rounds; + if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds; + if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds; if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; } @@ -54,126 +88,8 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps + current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } -// Function synchronizing current sense with motor driver. -// for in-line sensig no such thing is necessary -int LowsideCurrentSense::driverSync(BLDCDriver *driver){ - _driverSyncLowSide(); - return 1; -} - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(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 = 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.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = fabs(c.b/c.a); - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - - // if phase C measured - if(_isset(pinC)){ - // set phase B active and phases A and C down - driver->setPwm(0, 0, voltage); - _delay(200); - c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 50; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0f; - } - driver->setPwm(0, 0, 0); - gain_c *= _sign(c.c); - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 057aa623..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" @@ -21,33 +23,22 @@ class LowsideCurrentSense: public CurrentSense{ @param phB B phase adc pin @param phC C phase adc pin (optional) */ - LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = _NC); + /** + LowsideCurrentSense class constructor + @param mVpA mV per Amp ratio + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowsideCurrentSense(float mVpA, int pinA, int pinB, int pinC = _NC); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; - - // 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 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 @@ -57,9 +48,6 @@ class LowsideCurrentSense: public CurrentSense{ * Function finding zero offsets of the ADC */ void calibrateOffsets(); - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index e1b50003..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -4,30 +4,46 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +// flag returned if current sense init fails +#define SIMPLEFOC_CURRENT_SENSE_INIT_FAILED ((void*)-1) + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct GenericCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; +} GenericCurrentSenseParams; + + /** * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageInline(const int pinA); +float _readADCVoltageInline(const int pinA, const void* cs_params); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCInline(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCLowSide(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); void _startADC3PinConversionLowSide(); @@ -35,12 +51,18 @@ void _startADC3PinConversionLowSide(); * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageLowSide(const int pinA); +float _readADCVoltageLowSide(const int pinA, const void* cs_params); /** * function syncing the Driver with the ADC for the LowSide Sensing + * @param driver_params - driver parameter structure - hardware specific + * @param cs_params - current sense parameter structure - hardware specific + * + * @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* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 75265c03..50ae5965 100644 --- a/src/current_sense/hardware_specific/atmega_mcu.cpp +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -5,10 +5,6 @@ #define _ADC_VOLTAGE 5.0f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - #ifndef cbi #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #endif @@ -16,40 +12,29 @@ #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) #endif -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); +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); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample cbi(ADCSRA, ADPS2); sbi(ADCSRA, ADPS1); sbi(ADCSRA, ADPS0); -} - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + return params; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp index ef450b35..ce58cd9c 100644 --- a/src/current_sense/hardware_specific/due_mcu.cpp +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -5,34 +5,23 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); +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); -} + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); + return params; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp new file mode 100644 index 00000000..caf29c19 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -0,0 +1,176 @@ + +#include "esp32_mcu.h" +#include "esp32_adc_driver.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 + + +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant + +#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); + + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== +} + + +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){ + 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 - 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); + } + + uint16_t value = 0; + + if(channel > 7){ + //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 { + //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); + } + + // return value + return value; +} + +#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 + } + + // start the ADC conversion + if(channel > 9){ + 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_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 > 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 { + //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); + } + + 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 diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h new file mode 100644 index 00000000..cad441fc --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -0,0 +1,24 @@ +#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +/** + * Get ADC value for pin + * @param pin - pin number + * @return ADC value (0 - 4095) + * */ +uint16_t adcRead(uint8_t pin); + +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin + */ +bool adcInit(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_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 new file mode 100644 index 00000000..e5ed3fbf --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -0,0 +1,38 @@ +#include "esp32_mcu.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((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){ + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + // 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; + } + } + } + + return params; +} + + +#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_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp deleted file mode 100644 index 76ee54e9..00000000 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ /dev/null @@ -1,225 +0,0 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -#include "esp_intr.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits - - SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) - | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) - | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ - - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} - -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ - if(!__adcAttachPin(pin) || !__adcStart(pin)){ - return 0; - } - return __adcEnd(pin); -} - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h deleted file mode 100644 index df8166f0..00000000 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ /dev/null @@ -1,88 +0,0 @@ - - -#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) -/* - * Get ADC value for pin - * */ -uint16_t adcRead(uint8_t pin); - -/* - * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). - * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. - * Range is 1 - 16 - * - * Note: compatibility with Arduino SAM - */ -void __analogReadResolution(uint8_t bits); - -/* - * Sets the sample bits and read resolution - * Default is 12bit (0 - 4095) - * Range is 9 - 12 - * */ -void __analogSetWidth(uint8_t bits); - -/* - * Set number of cycles per sample - * Default is 8 and seems to do well - * Range is 1 - 255 - * */ -void __analogSetCycles(uint8_t cycles); - -/* - * Set number of samples in the range. - * Default is 1 - * Range is 1 - 255 - * This setting splits the range into - * "samples" pieces, which could look - * like the sensitivity has been multiplied - * that many times - * */ -void __analogSetSamples(uint8_t samples); - -/* - * Set the divider for the ADC clock. - * Default is 1 - * Range is 1 - 255 - * */ -void __analogSetClockDiv(uint8_t clockDiv); - -/* - * Set the attenuation for all channels - * Default is 11db - * */ -void __analogSetAttenuation(uint8_t attenuation); - -/* - * Set the attenuation for particular pin - * Default is 11db - * */ -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); - -/* - * Attach pin to ADC (will also clear any other analog mode that could be on) - * */ -bool __adcAttachPin(uint8_t pin); - -/* - * Start ADC conversion on attached pin's bus - * */ -bool __adcStart(uint8_t pin); - -/* - * Check if conversion on the pin's ADC bus is currently running - * */ -bool __adcBusy(uint8_t pin); - -/* - * Get the result of the conversion (will wait if it have not finished) - * */ -uint16_t __adcEnd(uint8_t pin); - -#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ -#endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp deleted file mode 100644 index 7b9ba448..00000000 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "../hardware_api.h" -#include "../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#include -#include - -#include "esp32_adc_driver.h" - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = adcRead(pinA); - // uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} - -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB, const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - - -/** - * Low side adc reading implementation -*/ -static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; -int a1, a2, a3; // Current readings from internal current sensor amplifiers -int _pinA, _pinB, _pinC; -static void IRAM_ATTR isr_handler(void*); -byte currentState = 1; - - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin){ - uint32_t raw_adc; - - if (pin == _pinA) raw_adc = a1; - else if (pin == _pinB) raw_adc = a2; - else if (pin == _pinC) raw_adc = a3; - - return raw_adc * _ADC_CONV; -} - -// function reading an ADC value and returning the read voltage -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - _pinA = pinA; - _pinB = pinB; - _pinC = pinC; - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - -void _driverSyncLowSide(){ - // high side registers enable interrupt - // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt - - // low-side register enable interrupt - MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt - if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt - mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -// Read currents when interrupt is triggered -static void IRAM_ATTR isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; - // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; - // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; - - // low side - uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; - uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; - uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0; - - switch (currentState) - { - case 1 : - if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); - currentState = 2; - break; - case 2 : - if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); - currentState = _isset(_pinC) ? 3 : 1; - break; - case 3 : - if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); - currentState = 1; - break; - } - - // high side - // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; - // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; - // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; - // low side - MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; - MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; - if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; -} - - -#endif diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 50f383ad..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,31 +1,47 @@ #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){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * 5.0f/1024.0f; + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); +__attribute__((weak)) 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); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (5.0f)/(1024.0f) + }; + + return params; } // function reading an ADC value and returning the read voltage -__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +__attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + 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 int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); +__attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(){ } +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ + _UNUSED(driver_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 new file mode 100644 index 00000000..d2ed881b --- /dev/null +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -0,0 +1,265 @@ + +#if defined(TARGET_RP2040) + + +#include "../../hardware_api.h" +#include "./rp2040_mcu.h" +#include "../../../drivers/hardware_specific/rp2040/rp2040_mcu.h" +#include "communication/SimpleFOCDebug.h" + +#include "hardware/dma.h" +#include "hardware/irq.h" +#include "hardware/pwm.h" +#include "hardware/adc.h" + + +/* Singleton instance of the ADC engine */ +RP2040ADCEngine engine; + +alignas(32) const uint32_t trigger_value = ADC_CS_START_ONCE_BITS; // start once + +/* Hardware API implementation */ + +float _readADCVoltageInline(const int pinA, const void* cs_params) { + // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to + // 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; + } + + // otherwise return NaN + return NAN; +}; + + +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) ) + engine.addPin(pinB); + if( _isset(pinC) ) + engine.addPin(pinC); + engine.init(); // TODO this has to happen later if we want to support more than one motor... + engine.start(); + return &engine; +}; + + +// not supported at the moment +// void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC) { +// if( _isset(pinA) ) +// engine.addPin(pinA); +// if( _isset(pinB) ) +// engine.addPin(pinB); +// if( _isset(pinC) ) +// engine.addPin(pinC); +// engine.setPWMTrigger(((RP2040DriverParams*)driver_params)->slice[0]); +// engine.init(); +// engine.start(); +// return &engine; +// }; + + +// void _startADC3PinConversionLowSide() { +// // what is this for? +// }; + + +// float _readADCVoltageLowSide(const int pinA, const void* cs_params) { +// // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to +// // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-( +// // like this we have block interrupts 3x instead of just once, and of course have the chance of reading across +// // new ADC conversions, which probably won't improve the accuracy. + +// if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) { +// return engine.lastResults[pinA-26]*engine.adc_conv; +// } + +// // otherwise return NaN +// return NAN; +// }; + + +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { +// // nothing to do +// }; + + + +volatile int rp2040_intcount = 0; + +void _adcConversionFinishedHandler() { + // conversion of all channels finished. copy results. + volatile uint8_t* from = engine.samples; + if (engine.channelsEnabled[0]) + engine.lastResults.raw[0] = (*from++); + if (engine.channelsEnabled[1]) + engine.lastResults.raw[1] = (*from++); + if (engine.channelsEnabled[2]) + engine.lastResults.raw[2] = (*from++); + if (engine.channelsEnabled[3]) + engine.lastResults.raw[3] = (*from++); + //dma_channel_acknowledge_irq0(engine.readDMAChannel); + dma_hw->ints0 = 1u << engine.readDMAChannel; + //dma_start_channel_mask( (1u << engine.readDMAChannel) ); + dma_channel_set_write_addr(engine.readDMAChannel, engine.samples, true); + // if (engine.triggerPWMSlice>=0) + // dma_channel_set_trans_count(engine.triggerDMAChannel, 1, true); + rp2040_intcount++; +}; + + + +/* ADC engine implementation */ + + +RP2040ADCEngine::RP2040ADCEngine() { + channelsEnabled[0] = false; + channelsEnabled[1] = false; + channelsEnabled[2] = false; + channelsEnabled[3] = false; + initialized = false; +}; + + + +void RP2040ADCEngine::addPin(int pin) { + if (pin>=26 && pin<=29) + channelsEnabled[pin-26] = true; + else + SIMPLEFOC_DEBUG("RP2040-CUR: ERR: Not an ADC pin: ", pin); +}; + + + +// void RP2040ADCEngine::setPWMTrigger(uint slice){ +// triggerPWMSlice = slice; +// }; + + + + +bool RP2040ADCEngine::init() { + if (initialized) + return true; + + adc_init(); + int enableMask = 0x00; + int channelCount = 0; + for (int i = 3; i>=0; i--) { + if (channelsEnabled[i]){ + adc_gpio_init(i+26); + enableMask |= (0x01<=500000) { + samples_per_second = 0; + adc_set_clkdiv(0); + } + else + adc_set_clkdiv(48000000/samples_per_second); + SIMPLEFOC_DEBUG("RP2040-CUR: ADC init"); + + readDMAChannel = dma_claim_unused_channel(true); + dma_channel_config cc1 = dma_channel_get_default_config(readDMAChannel); + channel_config_set_transfer_data_size(&cc1, DMA_SIZE_8); + channel_config_set_read_increment(&cc1, false); + channel_config_set_write_increment(&cc1, true); + channel_config_set_dreq(&cc1, DREQ_ADC); + channel_config_set_irq_quiet(&cc1, false); + dma_channel_configure(readDMAChannel, + &cc1, + samples, // dest + &adc_hw->fifo, // source + channelCount, // count + false // defer start + ); + dma_channel_set_irq0_enabled(readDMAChannel, true); + irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY); + + SIMPLEFOC_DEBUG("RP2040-CUR: DMA init"); + + // if (triggerPWMSlice>=0) { // if we have a trigger + // triggerDMAChannel = dma_claim_unused_channel(true); + // dma_channel_config cc3 = dma_channel_get_default_config(triggerDMAChannel); + // channel_config_set_transfer_data_size(&cc3, DMA_SIZE_32); + // channel_config_set_read_increment(&cc3, false); + // channel_config_set_write_increment(&cc3, false); + // channel_config_set_irq_quiet(&cc3, true); + // channel_config_set_dreq(&cc3, DREQ_PWM_WRAP0+triggerPWMSlice); //pwm_get_dreq(triggerPWMSlice)); + // pwm_set_irq_enabled(triggerPWMSlice, true); + // dma_channel_configure(triggerDMAChannel, + // &cc3, + // hw_set_alias_untyped(&adc_hw->cs), // dest + // &trigger_value, // source + // 1, // count + // true // defer start + // ); + // SIMPLEFOC_DEBUG("RP2040-CUR: PWM trigger init slice ", triggerPWMSlice); + // } + + initialized = true; + return initialized; +}; + + + + +void RP2040ADCEngine::start() { + SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine starting"); + irq_set_enabled(DMA_IRQ_0, true); + dma_start_channel_mask( (1u << readDMAChannel) ); + for (int i=0;i<4;i++) { + if (channelsEnabled[i]) { + adc_select_input(i); // set input to first enabled channel + break; + } + } + // if (triggerPWMSlice>=0) { + // dma_start_channel_mask( (1u << triggerDMAChannel) ); + // //hw_set_bits(&adc_hw->cs, trigger_value); + // } + // else + adc_run(true); + SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine started"); +}; + + + + +void RP2040ADCEngine::stop() { + adc_run(false); + irq_set_enabled(DMA_IRQ_0, false); + dma_channel_abort(readDMAChannel); + // if (triggerPWMSlice>=0) + // dma_channel_abort(triggerDMAChannel); + adc_fifo_drain(); + SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine stopped"); +}; + + + +ADCResults RP2040ADCEngine::getLastResults() { + ADCResults r; + r.value = lastResults.value; + return r; +}; + + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h new file mode 100644 index 00000000..ae28bb26 --- /dev/null +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h @@ -0,0 +1,93 @@ + + +#pragma once + +/* + * RP2040 ADC features are very weak :-( + * - only 4 inputs + * - only 9 bit effective resolution + * - read only 1 input at a time + * - 2 microseconds conversion time! + * - no triggers from PWM / events, only DMA + * + * So to read 3 phases takes 6 microseconds. :-( + * + * The RP2040 ADC engine takes over the control of the MCU's ADC. Parallel ADC access is not permitted, as this would + * cause conflicts with the engine's DMA based access and cause crashes. + * To use the other ADC channels, use them via this engine. Use addPin() to add them to the conversion, and getLastResult() + * to retrieve their value at any time. + * + * For motor current sensing, the engine supports inline sensing only. + * + * 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 + * sensing to work correctly, all PWM slices have to be set to the same PWM frequency. + * In theory, two motors could be sensed using 2 shunts on each motor. + * + * Note that if using other ADC channels along with the motor current sensing, those channels will be subject to the same conversion schedule as the motor's ADC channels, i.e. convert at the same fixed rate in case + * of inline sensing. + * + * Solution to trigger ADC conversion from PWM via DMA: + * use the PWM wrap as a DREQ to a DMA channel, and have the DMA channel write to the ADC's CS register to trigger an ADC sample. + * Unfortunately, I could not get this to work, so no low side sensing for the moment. + * + * Solution for ADC conversion: + * ADC converts all channels in round-robin mode, and writes to FIFO. FIFO is emptied by a DMA which triggers after N conversions, + * where N is the number of ADC channels used. So this DMA copies all the values from one round-robin conversion. + * + * + */ + + +#define SIMPLEFOC_RP2040_ADC_RESOLUTION 256 +#ifndef SIMPLEFOC_RP2040_ADC_VDDA +#define SIMPLEFOC_RP2040_ADC_VDDA 3.3f +#endif + + +union ADCResults { + uint32_t value; + uint8_t raw[4]; + struct { + uint8_t ch0; + uint8_t ch1; + uint8_t ch2; + uint8_t ch3; + }; +}; + + +class RP2040ADCEngine { + +public: + RP2040ADCEngine(); + void addPin(int pin); + //void setPWMTrigger(uint slice); + + bool init(); + void start(); + void stop(); + + ADCResults getLastResults(); // TODO find a better API and representation for this + + 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; + bool initialized; + uint readDMAChannel; + //uint copyDMAChannel; + //uint triggerDMAChannel; + + bool channelsEnabled[4]; + volatile uint8_t samples[4]; + volatile ADCResults lastResults; + //alignas(32) volatile uint8_t nextResults[4]; +}; \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp similarity index 92% rename from src/current_sense/hardware_specific/samd21_mcu.cpp rename to src/current_sense/hardware_specific/samd/samd21_mcu.cpp index 8bb6d38c..046f3db4 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -1,28 +1,30 @@ #ifdef _SAMD21_ #include "samd21_mcu.h" -#include "../hardware_api.h" +#include "../../hardware_api.h" static bool freeRunning = false; static int _pinA, _pinB, _pinC; static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; -/** - * function reading an ADC value and returning the read voltage - * - * @param pinA - adc pin A - * @param pinB - adc pin B - * @param pinC - adc pin C - */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC) + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC) { + _UNUSED(driver_params); + _pinA = pinA; _pinB = pinB; _pinC = pinC; freeRunning = true; instance.init(pinA, pinB, pinC); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC } + }; + + return params; } void _startADC3PinConversionLowSide() { @@ -33,8 +35,10 @@ void _startADC3PinConversionLowSide() * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltageLowSide(const int pinA) +float _readADCVoltageLowSide(const int pinA, const void* cs_params) { + _UNUSED(cs_params); + instance.readResults(a, b, c); if(pinA == _pinA) @@ -50,11 +54,15 @@ float _readADCVoltageLowSide(const int pinA) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide() +void* _driverSyncLowSide(void* driver_params, void* cs_params) { + _UNUSED(driver_params); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts + + return cs_params; } @@ -137,7 +145,8 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { void SAMDCurrentSenseADCDMA::initPins(){ - pinMode(pinAREF, INPUT); + if (pinAREF>=0) + pinMode(pinAREF, INPUT); pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -169,8 +178,10 @@ void SAMDCurrentSenseADCDMA::initADC(){ //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default - ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; - // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + if (pinAREF>=0) + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + else + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; ADCsync(); // ref 31.6.16 /* diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd/samd21_mcu.h similarity index 96% rename from src/current_sense/hardware_specific/samd21_mcu.h rename to src/current_sense/hardware_specific/samd/samd21_mcu.h index c0cec74a..e7d74426 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.h @@ -3,7 +3,7 @@ #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H -// #define SIMPLEFOC_SAMD_DEBUG +#define SIMPLEFOC_SAMD_DEBUG #if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) #define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial #endif @@ -18,6 +18,8 @@ } dmacdescriptor ; +// AREF pin is 42 + class SAMDCurrentSenseADCDMA { @@ -64,4 +66,4 @@ class SAMDCurrentSenseADCDMA -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/samd/samd_mcu.cpp b/src/current_sense/hardware_specific/samd/samd_mcu.cpp new file mode 100644 index 00000000..2ec47c0d --- /dev/null +++ b/src/current_sense/hardware_specific/samd/samd_mcu.cpp @@ -0,0 +1,23 @@ +#include "../../hardware_api.h" + +#if defined(_SAMD21_)|| defined(_SAMD51_) || defined(_SAME51_) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.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); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp deleted file mode 100644 index 550d7bee..00000000 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "../hardware_api.h" - -#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 1024.0f - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp similarity index 82% rename from src/current_sense/hardware_specific/stm32g4_hal.cpp rename to src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp index f0204566..dc505d6f 100644 --- a/src/current_sense/hardware_specific/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp @@ -1,10 +1,13 @@ -#include "../hardware_api.h" -#if defined(STM32G4xx) +#include "../../../hardware_api.h" +#if defined(ARDUINO_B_G431B_ESC1) + +#include "communication/SimpleFOCDebug.h" #include "stm32g4xx_hal.h" #include "stm32g4xx_ll_pwr.h" #include "stm32g4xx_hal_adc.h" -#include "stm32g4_hal.h" +#include "b_g431_hal.h" + // From STM32 cube IDE /** ****************************************************************************** @@ -92,7 +95,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1->Init.LowPowerAutoWait = DISABLE; hadc1->Init.ContinuousConvMode = DISABLE; - hadc1->Init.NbrOfConversion = 2; + hadc1->Init.NbrOfConversion = 5; hadc1->Init.DiscontinuousConvMode = DISABLE; hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; @@ -101,7 +104,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) if (HAL_ADC_Init(hadc1) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure the ADC multi-mode @@ -109,7 +112,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) multimode.Mode = ADC_MODE_INDEPENDENT; if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!"); } /** Configure Regular Channel */ @@ -121,7 +124,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /** Configure Regular Channel */ @@ -129,7 +132,48 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Rank = ADC_REGULAR_RANK_2; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + //****************************************************************** + // Temp, Poti .... + /* Configure Regular Channel (PB12, Potentiometer) + */ + sConfig.Channel = ADC_CHANNEL_11; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PB14, Temperature) + */ + sConfig.Channel = ADC_CHANNEL_5; + sConfig.Rank = ADC_REGULAR_RANK_4; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PB14, Temperature) + */ + sConfig.Channel = ADC_CHANNEL_1; + sConfig.Rank = ADC_REGULAR_RANK_5; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /* USER CODE BEGIN ADC1_Init 2 */ @@ -173,7 +217,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) if (HAL_ADC_Init(hadc2) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure Regular Channel */ @@ -185,7 +229,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /* USER CODE BEGIN ADC2_Init 2 */ diff --git a/src/current_sense/hardware_specific/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h similarity index 67% rename from src/current_sense/hardware_specific/stm32g4_hal.h rename to src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h index 13dd1de3..2d6a1f0a 100644 --- a/src/current_sense/hardware_specific/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h @@ -1,7 +1,11 @@ -#ifndef stm32g4_hal -#define stm32g4_hal +#ifndef B_G431_ESC1_HAL +#define B_G431_ESC1_HAL + +#if defined(ARDUINO_B_G431B_ESC1) + +#include +#include -#if defined(STM32G4xx) void MX_GPIO_Init(void); void MX_DMA_Init(void); void MX_ADC1_Init(ADC_HandleTypeDef* hadc1); 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 new file mode 100644 index 00000000..46cb20be --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -0,0 +1,178 @@ +#include "../../../hardware_api.h" + +#if defined(ARDUINO_B_G431B_ESC1) + +#include "b_g431_hal.h" +#include "Arduino.h" +#include "../stm32_mcu.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "communication/SimpleFOCDebug.h" + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f +#define ADC_BUF_LEN_1 5 +#define ADC_BUF_LEN_2 1 + +static ADC_HandleTypeDef hadc1; +static ADC_HandleTypeDef hadc2; +static OPAMP_HandleTypeDef hopamp1; +static OPAMP_HandleTypeDef hopamp2; +static OPAMP_HandleTypeDef hopamp3; + +static DMA_HandleTypeDef hdma_adc1; +static DMA_HandleTypeDef hdma_adc2; + +volatile uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion +volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion + +// function reading an ADC value and returning the read voltage +// As DMA is being used just return the DMA result +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]; + else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer2[0]; +#ifdef PB1 + else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer1[0]; +#endif + + else if (pin == A_POTENTIOMETER) + raw_adc = adcBuffer1[2]; + else if (pin == A_TEMPERATURE) + raw_adc = adcBuffer1[3]; + else if (pin == A_VBUS) + raw_adc = adcBuffer1[4]; + + return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ + // could this be replaced with LL_OPAMP calls?? + hopamp->Instance = OPAMPx_Def; + hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; + hopamp->Init.Mode = OPAMP_PGA_MODE; + hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; + hopamp->Init.InternalOutput = DISABLE; + hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; + hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; + hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; + hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; + if (HAL_OPAMP_Init(hopamp) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!"); + } +} +void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ + // Configure the opamps + _configureOPAMP(OPAMPA, OPAMP1); + _configureOPAMP(OPAMPB, OPAMP2); + _configureOPAMP(OPAMPC, OPAMP3); +} + +void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) { + hdma_adc->Instance = channel; + hdma_adc->Init.Request = request; + hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE; + hdma_adc->Init.MemInc = DMA_MINC_ENABLE; + hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc->Init.Mode = DMA_CIRCULAR; + hdma_adc->Init.Priority = DMA_PRIORITY_LOW; + HAL_DMA_DeInit(hdma_adc); + if (HAL_DMA_Init(hdma_adc) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_DMA_Init failed!"); + } + __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); +} + +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + _UNUSED(pinA); + _UNUSED(pinB); + _UNUSED(pinC); + + SIMPLEFOC_DEBUG("B-G431B does not implement inline current sense. Use low-side current sense instead."); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; +} + + +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + HAL_Init(); + MX_GPIO_Init(); + MX_DMA_Init(); + _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2); + 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); + + if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) + { + SIMPLEFOC_DEBUG("DMA read init failed"); + } + if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) + { + SIMPLEFOC_DEBUG("DMA read init failed"); + } + + HAL_OPAMP_Start(&hopamp1); + HAL_OPAMP_Start(&hopamp2); + HAL_OPAMP_Start(&hopamp3); + + Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), + .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this) + }; + + return params; +} + +extern "C" { +void DMA1_Channel1_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc1); +} + +void DMA1_Channel2_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc2); +} +} + +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // 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. + // 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; + } + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // 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/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp new file mode 100644 index 00000000..94253d74 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp @@ -0,0 +1,33 @@ + +#include "../../hardware_api.h" + +#if defined(_STM32_DEF_) and !defined(ARDUINO_B_G431B_ESC1) + +#include "stm32_mcu.h" + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.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); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + +// function reading an ADC value and returning the read voltage +__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h new file mode 100644 index 00000000..6e238170 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -0,0 +1,21 @@ + +#ifndef STM32_CURRENTSENSE_MCU_DEF +#define STM32_CURRENTSENSE_MCU_DEF +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +#if defined(_STM32_DEF_) + +// 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 Stm32CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; + ADC_HandleTypeDef* adc_handle = NP; + HardwareTimer* timer_handle = NP; +} Stm32CurrentSenseParams; + +#endif +#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 new file mode 100644 index 00000000..d3bea81e --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -0,0 +1,157 @@ +#include "stm32f1_hal.h" + +#if defined(STM32F1xx) + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 +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 + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGCONV_T3_TRGO; +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +ADC_HandleTypeDef hadc; + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + /**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 + } + + hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc.Init.ContinuousConvMode = ENABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 1; + HAL_ADC_Init(&hadc); + /**Configure for the selected ADC regular channel to be converted. + */ + + /**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_1CYCLE_5; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + while(driver_params->timers[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + 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 = driver_params->timers[tim_num-1]; + // 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_REGULAR_RANK_1; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + // second channel + sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + } + + 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; + } + +} + +extern "C" { + void ADC1_2_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h new file mode 100644 index 00000000..b0f4f83b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h @@ -0,0 +1,17 @@ +#ifndef STM32F1_LOWSIDE_HAL +#define STM32F1_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32F1xx) +#include "stm32f1xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.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 + +#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 new file mode 100644 index 00000000..49f2f3d5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -0,0 +1,141 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F1xx) +#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 "stm32f1_hal.h" +#include "Arduino.h" + +#define _ADC_VOLTAGE_F1 3.3f +#define _ADC_RESOLUTION_F1 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] = {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 + else if(AdcHandle->Instance == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle->Instance == ADC3) return 2; +#endif + return 0; +} + +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_F1) / (_ADC_RESOLUTION_F1) + }; + _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; + }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 + 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 + 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 + 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp new file mode 100644 index 00000000..bd0df4b6 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -0,0 +1,166 @@ +#include "stm32f4_hal.h" + +#if defined(STM32F4xx) + +//#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_DIV4; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = ENABLE; + 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_RISING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + while(driver_params->timers[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + 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 = driver_params->timers[tim_num-1]; + // 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; + } + } + + 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; + } +} + +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h new file mode 100644 index 00000000..71071a56 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -0,0 +1,18 @@ +#ifndef STM32F4_LOWSIDE_HAL +#define STM32F4_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32F4xx) +#include "stm32f4xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32f4_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 + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp new file mode 100644 index 00000000..6b597d4e --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -0,0 +1,130 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F4xx) +#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 "stm32f4_hal.h" +#include "stm32f4_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE_F4 3.3f +#define _ADC_RESOLUTION_F4 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] = {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 { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4) + }; + _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; + }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 + 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 + 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 + 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp new file mode 100644 index 00000000..20793d8c --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -0,0 +1,193 @@ +#include "stm32f4_utils.h" + +#if defined(STM32F4xx) + +/* Exported Functions */ +/** + * @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; +} + + +// 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 + else + return _TRGO_NOT_AVAILABLE; +} + +// 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 == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGCONV_T3_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/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h new file mode 100644 index 00000000..b4549bad --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32F4_UTILS_HAL +#define STM32F4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32F4xx) + +#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 + +#endif \ No newline at end of file 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 new file mode 100644 index 00000000..fd1090ae --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -0,0 +1,231 @@ +#include "stm32g4_hal.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#include "../../../../communication/SimpleFOCDebug.h" + +#define SIMPLEFOC_STM32_DEBUG + +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) { +#ifdef __HAL_RCC_ADC1_CLK_ENABLE + __HAL_RCC_ADC1_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#ifdef ADC2 + else if (hadc.Instance == ADC2) { +#ifdef __HAL_RCC_ADC2_CLK_ENABLE + __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC3 + else if (hadc.Instance == ADC3) { +#ifdef __HAL_RCC_ADC3_CLK_ENABLE + __HAL_RCC_ADC3_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { +#ifdef __HAL_RCC_ADC4_CLK_ENABLE + __HAL_RCC_ADC4_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#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_DIV4; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.LowPowerAutoWait = DISABLE; + hadc.Init.GainCompensation = 0; + 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; + hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; + 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_2CYCLES_5; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjecOversamplingMode = DISABLE; + sConfigInjected.QueueInjectedContext = DISABLE; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + while(driver_params->timers[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + 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 = driver_params->timers[tim_num-1]; + // 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; + } + + + // 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; + } + } + + 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; + } +} + +extern "C" { + void ADC1_2_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#ifdef ADC3 + void ADC3_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC4 + void ADC4_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC5 + void ADC5_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h new file mode 100644 index 00000000..2298b17c --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -0,0 +1,19 @@ +#ifndef STM32G4_LOWSIDE_HAL +#define STM32G4_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#include "stm32g4xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32g4_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 + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp new file mode 100644 index 00000000..9c73f6d7 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -0,0 +1,167 @@ +#include "../../../hardware_api.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#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 "stm32g4_hal.h" +#include "stm32g4_utils.h" +#include "Arduino.h" + +// #define SIMPLEFOC_STM32_ADC_INTERRUPT + +#define _ADC_VOLTAGE_G4 3.3f +#define _ADC_RESOLUTION_G4 4096.0f + + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; +// does adc interrupt need a downsample - per adc (5) +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){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4) + }; + _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; + }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,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 + 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 + 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp new file mode 100644 index 00000000..89a9bc34 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -0,0 +1,237 @@ +#include "stm32g4_utils.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +/* Exported Functions */ +/** + * @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; +} + + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM7 // if defined timer 7 + else if(timer->getHandle()->Instance == TIM7) + return ADC_EXTERNALTRIGINJEC_T7_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif +#ifdef TIM20 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM20) + return ADC_EXTERNALTRIGINJEC_T20_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM7 // if defined timer 7 + else if(timer->getHandle()->Instance == TIM7) + return ADC_EXTERNALTRIG_T7_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIG_T7_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_TRGO; +#endif +#ifdef TIM20 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM20) + return ADC_EXTERNALTRIG_T20_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/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h new file mode 100644 index 00000000..fa857bd0 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32G4_UTILS_HAL +#define STM32G4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#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/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp new file mode 100644 index 00000000..67a0473b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -0,0 +1,230 @@ +#include "stm32l4_hal.h" + +#if defined(STM32L4xx) + +#include "../../../../communication/SimpleFOCDebug.h" + +#define SIMPLEFOC_STM32_DEBUG + +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) { +#ifdef __HAL_RCC_ADC1_CLK_ENABLE + __HAL_RCC_ADC1_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#ifdef ADC2 + else if (hadc.Instance == ADC2) { +#ifdef __HAL_RCC_ADC2_CLK_ENABLE + __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC3 + else if (hadc.Instance == ADC3) { +#ifdef __HAL_RCC_ADC3_CLK_ENABLE + __HAL_RCC_ADC3_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { +#ifdef __HAL_RCC_ADC4_CLK_ENABLE + __HAL_RCC_ADC4_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#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_DIV4; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.LowPowerAutoWait = 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; + hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; + 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_2CYCLES_5; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjecOversamplingMode = DISABLE; + sConfigInjected.QueueInjectedContext = DISABLE; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + while(driver_params->timers[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + 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 = driver_params->timers[tim_num-1]; + // 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; + } + + + // 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; + } + } + + 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; + } +} + +extern "C" { + void ADC1_2_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#ifdef ADC3 + void ADC3_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC4 + void ADC4_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC5 + void ADC5_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h new file mode 100644 index 00000000..0317b74b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -0,0 +1,19 @@ +#ifndef STM32L4_LOWSIDE_HAL +#define STM32L4_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32L4xx) + +#include "stm32l4xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32l4_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 + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp new file mode 100644 index 00000000..5de6432a --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -0,0 +1,163 @@ +#include "../../../hardware_api.h" + +#if defined(STM32L4xx) + +#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 "stm32l4_hal.h" +#include "stm32l4_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE_L4 3.3f +#define _ADC_RESOLUTION_L4 4096.0f + + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; +// does adc interrupt need a downsample - per adc (5) +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){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4) + }; + _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; + }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,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 + 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 + 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp new file mode 100644 index 00000000..376d9d68 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -0,0 +1,221 @@ +#include "stm32l4_utils.h" + +#if defined(STM32L4xx) + +/* Exported Functions */ +/** + * @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; +} + + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_adc_ex.h#L210 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIG_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_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/stm32l4/stm32l4_utils.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h new file mode 100644 index 00000000..ceef9be7 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32L4_UTILS_HAL +#define STM32L4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32L4xx) + +#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/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp deleted file mode 100644 index 797ac6a1..00000000 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ /dev/null @@ -1,39 +0,0 @@ - -#include "../hardware_api.h" - -#if defined(_STM32_DEF_) and !defined(STM32G4xx) - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 1024.0f - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32g4_mcu.cpp deleted file mode 100644 index 5b5d00ee..00000000 --- a/src/current_sense/hardware_specific/stm32g4_mcu.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "../hardware_api.h" -#include "stm32g4_hal.h" - -#if defined(STM32G4xx) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4096.0 -#define ADC_BUF_LEN_1 2 -#define ADC_BUF_LEN_2 1 - -static ADC_HandleTypeDef hadc1; -static ADC_HandleTypeDef hadc2; -static OPAMP_HandleTypeDef hopamp1; -static OPAMP_HandleTypeDef hopamp2; -static OPAMP_HandleTypeDef hopamp3; - -static DMA_HandleTypeDef hdma_adc1; -static DMA_HandleTypeDef hdma_adc2; - -uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion -uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion - -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -// As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin){ - uint32_t raw_adc = 0; - if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 - raw_adc = adcBuffer1[1]; - else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 - raw_adc = adcBuffer2[0]; - else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 - raw_adc = adcBuffer1[0]; - - return raw_adc * _ADC_CONV; -} -// do the same for low side sensing -float _readADCVoltageLowSide(const int pin){ - return _readADCVoltageInline(pin); -} - -void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ - // could this be replaced with LL_OPAMP calls?? - hopamp->Instance = OPAMPx_Def; - hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; - hopamp->Init.Mode = OPAMP_PGA_MODE; - hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; - hopamp->Init.InternalOutput = DISABLE; - hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; - hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; - hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; - hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; - if (HAL_OPAMP_Init(hopamp) != HAL_OK) - { - Error_Handler(); - } -} -void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ - // Configure the opamps - _configureOPAMP(OPAMPA, OPAMP1); - _configureOPAMP(OPAMPB, OPAMP2); - _configureOPAMP(OPAMPC, OPAMP3); -} - -void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) { - hdma_adc->Instance = channel; - hdma_adc->Init.Request = request; - hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY; - hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE; - hdma_adc->Init.MemInc = DMA_MINC_ENABLE; - hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; - hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; - hdma_adc->Init.Mode = DMA_CIRCULAR; - hdma_adc->Init.Priority = DMA_PRIORITY_LOW; - HAL_DMA_DeInit(hdma_adc); - if (HAL_DMA_Init(hdma_adc) != HAL_OK) - { - Error_Handler(); - } - __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); -} - -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - HAL_Init(); - MX_GPIO_Init(); - MX_DMA_Init(); - _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2); - MX_ADC1_Init(&hadc1); - MX_ADC2_Init(&hadc2); - - MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); - MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); - - if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) - { - Error_Handler(); - } - if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) - { - Error_Handler(); - } - - HAL_OPAMP_Start(&hopamp1); - HAL_OPAMP_Start(&hopamp2); - HAL_OPAMP_Start(&hopamp3); - - // Check if the ADC DMA is collecting any data. - // If this fails, it likely means timer1 has not started. Verify that your application starts - // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. - _delay(5); - if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { - Error_Handler(); - } -} -// do the same for low side -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - _configureADCInline(pinA, pinB, pinC); -} - -extern "C" { -void DMA1_Channel1_IRQHandler(void) { - HAL_DMA_IRQHandler(&hdma_adc1); -} - -void DMA1_Channel2_IRQHandler(void) { - HAL_DMA_IRQHandler(&hdma_adc2); -} -} - -#endif \ No newline at end of file diff --git a/src/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/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp new file mode 100644 index 00000000..7669edc8 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -0,0 +1,24 @@ +#include "../../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.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); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp deleted file mode 100644 index 606ce618..00000000 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "../hardware_api.h" - -#if defined(__arm__) && defined(CORE_TEENSY) - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 1024.0f - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} -// function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); -} -// Configure low side for generic mcu -// cannot do much but -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); -} - -#endif \ No newline at end of file diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index bc239875..637c8db5 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -56,19 +56,20 @@ int BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); - return 0; + params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } // Set voltage to the pwm pin -void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver3PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) { // disable if needed if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ - digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); - digitalWrite(enableB_pin, sb == _HIGH_IMPEDANCE ? LOW : HIGH); - digitalWrite(enableC_pin, sc == _HIGH_IMPEDANCE ? LOW : HIGH); + digitalWrite(enableA_pin, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enableB_pin, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enableC_pin, sc == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); } } @@ -87,5 +88,5 @@ void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, params); } diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index d6a83590..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,15 +49,15 @@ 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 ) * @param sa - phase C state : active / disabled ( high impedance ) */ - virtual void setPhaseState(int sa, int sb, int sc) override; + virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) override; private: - }; diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index eaeb2cfa..4981858f 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -26,6 +26,8 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h void BLDCDriver6PWM::enable(){ // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); + // set phase state enabled + setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // set zero to PWM setPwm(0, 0, 0); } @@ -33,6 +35,8 @@ void BLDCDriver6PWM::enable(){ // disable motor driver void BLDCDriver6PWM::disable() { + // set phase state to disabled + setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_OFF, PhaseState::PHASE_OFF); // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available @@ -56,11 +60,22 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + // set phase state to disabled + phase_state[0] = PhaseState::PHASE_OFF; + phase_state[1] = PhaseState::PHASE_OFF; + phase_state[2] = PhaseState::PHASE_OFF; + + // set zero to PWM + dc_a = dc_b = dc_c = 0; + // configure 6pwm // hardware specific function - depending on driver and mcu - return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } + // Set voltage to the pwm pin void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver @@ -74,11 +89,15 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu - _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, phase_state, params); } -// Set voltage to the pwm pin -void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { - // TODO implement disabling +// Set the phase state +// actually changing the state is only done on the next call to setPwm, and depends +// on the hardware capabilities of the driver and MCU. +void BLDCDriver6PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) { + phase_state[0] = sa; + phase_state[1] = sb; + phase_state[2] = sc; } diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index cee37d64..7ba7631c 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,10 +37,12 @@ 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] + PhaseState phase_state[3]; //!< phase state (active / disabled) + + /** * Set phase voltages to the harware * @@ -57,7 +59,7 @@ class BLDCDriver6PWM: public BLDCDriver * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) */ - virtual void setPhaseState(int sa, int sb, int sc) override; + virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) override; private: diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a0e61ca0..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); } @@ -79,10 +79,19 @@ int StepperDriver2PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure2PWM(pwm_frequency, pwm1, pwm2); - return 0; + params = _configure2PWM(pwm_frequency, pwm1, pwm2); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + 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) { @@ -102,5 +111,5 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); // write to hardware - _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params); } \ No newline at end of file diff --git a/src/drivers/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 6c905348..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); } @@ -54,8 +54,18 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu - _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); - return 0; + params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); + 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); + } } @@ -76,5 +86,5 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { else duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware - _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); } \ No newline at end of file diff --git a/src/drivers/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 0b4f7c65..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -3,6 +3,53 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../communication/SimpleFOCDebug.h" +#include "../common/base_classes/BLDCDriver.h" + + +// these defines determine the polarity of the PWM output. Normally, the polarity is active-high, +// i.e. a high-level PWM output is expected to switch on the MOSFET. But should your driver design +// require inverted polarity, you can change the defines below, or set them via your build environment +// or board definition files. + +// used for 1-PWM, 2-PWM, 3-PWM, and 4-PWM modes +#ifndef SIMPLEFOC_PWM_ACTIVE_HIGH +#define SIMPLEFOC_PWM_ACTIVE_HIGH true +#endif +// used for 6-PWM mode, high-side +#ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true +#endif +// used for 6-PWM mode, low-side +#ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true +#endif + +// flag returned if driver init fails +#define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) + +// generic implementation of the hardware specific structure +// containing all the necessary driver parameters +// will be returned as a void pointer from the _configurexPWM functions +// will be provided to the _writeDutyCyclexPWM() as a void pointer +typedef struct GenericDriverParams { + int pins[6]; + long pwm_frequency; + float dead_zone; +} GenericDriverParams; + + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA pwm pin + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful + */ +void* _configure1PWM(long pwm_frequency, const int pinA); /** * Configuring PWM frequency, resolution and alignment @@ -12,8 +59,10 @@ * @param pwm_frequency - frequency in hertz - if applicable * @param pinA pinA bldc driver * @param pinB pinB bldc driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB); /** * Configuring PWM frequency, resolution and alignment @@ -24,8 +73,10 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); * @param pinA pinA bldc driver * @param pinB pinB bldc driver * @param pinC pinC bldc driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); /** * Configuring PWM frequency, resolution and alignment @@ -37,8 +88,10 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in * @param pin1B pin1B stepper driver * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); /** * Configuring PWM frequency, resolution and alignment @@ -54,9 +107,20 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const * @param pinC_h pinA high-side bldc driver * @param pinC_l pinA low-side bldc driver * - * @return 0 if config good, -1 if failed + * @return -1 if failed, or pointer to internal driver parameters struct if successful */ -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param params the driver parameters + */ +void _writeDutyCycle1PWM(float dc_a, void* params); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -65,10 +129,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const * * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -78,11 +141,9 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] * @param dc_c duty cycle phase C [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - * @param pinC phase C hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params); /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) @@ -93,12 +154,9 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB * @param dc_1b duty cycle phase 1B [0, 1] * @param dc_2a duty cycle phase 2A [0, 1] * @param dc_2b duty cycle phase 2B [0, 1] - * @param pin1A phase 1A hardware pin number - * @param pin1B phase 1B hardware pin number - * @param pin2A phase 2A hardware pin number - * @param pin2B phase 2B hardware pin number + * @param params the driver parameters */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params); /** @@ -109,15 +167,11 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in * @param dc_a duty cycle phase A [0, 1] * @param dc_b duty cycle phase B [0, 1] * @param dc_c duty cycle phase C [0, 1] - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - * @param pinA_h phase A high-side hardware pin number - * @param pinA_l phase A low-side hardware pin number - * @param pinB_h phase B high-side hardware pin number - * @param pinB_l phase B low-side hardware pin number - * @param pinC_h phase C high-side hardware pin number - * @param pinC_l phase C low-side hardware pin number + * @param phase_state pointer to PhaseState[3] array + * @param params the driver parameters * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params); + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp new file mode 100644 index 00000000..532b3ce3 --- /dev/null +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -0,0 +1,282 @@ +#include "../../hardware_api.h" + +#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 + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin, const long frequency){ + bool high_fq = false; + // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz) + // else set the 4kHz + if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true; + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-ATmega2560 + // https://forum.arduino.cc/index.php?topic=72092.0 + if (pin == 13 || pin == 4 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + } + else if (pin == 12 || pin == 11 ) + if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + else if (pin == 10 || pin == 9 ) + if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + else if (pin == 5 || pin == 3 || pin == 2) + if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + else if (pin == 8 || pin == 7 || pin == 6) + if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + else if (pin == 44 || pin == 45 || pin == 46) + if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware specific +// supports Arduino/ATmega2560 +void* _configure1PWM(long pwm_frequency,const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA, pwm_frequency); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware specific +// supports Arduino/ATmega2560 +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA, pwm_frequency); + _pinHighFrequency(pinB, pwm_frequency); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware specific +// supports Arduino/ATmega2560 +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 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA, pwm_frequency); + _pinHighFrequency(pinB, pwm_frequency); + _pinHighFrequency(pinC, pwm_frequency); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + // _syncAllTimers(); + return params; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - 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); +} + +// 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){ + // 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); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - 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); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware specific +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A,pwm_frequency); + _pinHighFrequency(pin1B,pwm_frequency); + _pinHighFrequency(pin2A,pwm_frequency); + _pinHighFrequency(pin2B,pwm_frequency); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin1B, pin2A, pin2B }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; +} + +// 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) { + // 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); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +// supports Arduino/ATmega2560 +// https://ww1.microchip.com/downloads/en/devicedoc/atmel-2549-8-bit-avr-microcontroller-atmega640-1280-1281-2560-2561_datasheet.pdf +// https://docs.arduino.cc/hacking/hardware/PinMapping2560 +int _configureComplementaryPair(const int pinH,const int pinL, long frequency) { + bool high_fq = false; + // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz) + // else set the 4kHz + if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true; + + // configure pin pairs + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set frequency + if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){ + // set frequency + if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){ + // set frequency + if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else if((pinH == 5 && pinL == 2 ) || (pinH == 2 && pinL == 5 ) ){ + // set frequency + if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 5 ) TCCR3A = 0b10110000 | (TCCR3A & 0b00001111) ; + else TCCR3A = 0b11100000 | (TCCR3A & 0b00001111) ; + }else if((pinH == 6 && pinL == 7 ) || (pinH == 7 && pinL == 6 ) ){ + // set frequency + if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 6 ) TCCR4A = 0b10110000 | (TCCR4A & 0b00001111) ; + else TCCR4A = 0b11100000 | (TCCR4A & 0b00001111) ; + }else if((pinH == 46 && pinL == 45 ) || (pinH == 45 && pinL == 46 ) ){ + // set frequency + if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 46 ) TCCR5A = 0b10110000 | (TCCR5A & 0b00001111) ; + else TCCR5A = 0b11100000 | (TCCR5A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - setting +// - hardware specific +// supports Arduino/ATmega2560 +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 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l, pwm_frequency); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l, pwm_frequency); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l, pwm_frequency); + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + // _syncAllTimers(); + return params; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time, PhaseState ps) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + digitalWrite(pinH, LOW); + }else{ + analogWrite(pinH, pwm_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + digitalWrite(pinL, LOW); + }else{ + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); + } + +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arduino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[0]); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[1]); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[2]); +} + +#endif diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp new file mode 100644 index 00000000..9df9b8a7 --- /dev/null +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -0,0 +1,259 @@ +#include "../../hardware_api.h" + +#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 + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin, const long frequency){ + bool high_fq = false; + // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz) + // else set the 4kHz + if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true; + // High PWM frequency + // https://www.arxterra.com/9-atmega328p-timers/ + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + }else if (pin == 9 || pin == 10 ){ + if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + }else if (pin == 3 || pin == 11){ + if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + } +} + +void _syncAllTimers(){ + GTCCR = (1<pins[0], 255.0f*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){ + // 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); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - 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); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware specific +// supports Arduino/ATmega328 +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A,pwm_frequency); + _pinHighFrequency(pin1B,pwm_frequency); + _pinHighFrequency(pin2A,pwm_frequency); + _pinHighFrequency(pin2B,pwm_frequency); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin1B, pin2A, pin2B }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; +} + +// 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){ + // 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); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(const int pinH, const int pinL, long frequency) { + bool high_fq = false; + // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz) + // else set the 4kHz + if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true; + + // configure pins + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set frequency + if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set frequency + if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set frequency + if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz + else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - setting +// - hardware specific +// supports Arduino/ATmega328 +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l, pwm_frequency); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l, pwm_frequency); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l, pwm_frequency); + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + _syncAllTimers(); + return params; +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time, PhaseState ps) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + digitalWrite(pinH, LOW); + }else{ + analogWrite(pinH, pwm_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + digitalWrite(pinL, LOW); + }else{ + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); + } + + +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arduino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[0]); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[1]); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[2]); +} + +#endif diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp similarity index 58% rename from src/drivers/hardware_specific/atmega32u4_mcu.cpp rename to src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index 5a8abe2e..4cf454ae 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -1,8 +1,12 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #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 @@ -31,64 +35,108 @@ void _pinHighFrequency(const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle1PWM(float dc_a, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +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); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin1B, pin2A, pin2B }, + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -134,17 +182,21 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured - + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + return params; +} // function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) @@ -163,10 +215,12 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + + _UNUSED(phase_state); } #endif diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp deleted file mode 100644 index 2d3c0305..00000000 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include "../hardware_api.h" - -#if defined(__AVR_ATmega2560__) - -// set pwm frequency to 32KHz -void _pinHighFrequency(const int pin){ - // High PWM frequency - // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - // https://forum.arduino.cc/index.php?topic=72092.0 - if (pin == 13 || pin == 4 ) { - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - } - else if (pin == 12 || pin == 11 ) - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 10 || pin == 9 ) - TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 5 || pin == 3 || pin == 2) - TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 8 || pin == 7 || pin == 6) - TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 44 || pin == 45 || pin == 46) - TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 - -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); -} - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); - _pinHighFrequency(pinC); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); -} - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pin1A); - _pinHighFrequency(pin1B); - _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); -} - - -// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm -// supports Arudino/ATmega2560 -int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ - // configure the pwm phase-corrected mode - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); - // configure complementary pwm on low side - if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; - else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; - // set prescaler to 1 - 32kHz freq - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); - }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){ - // set prescaler to 1 - 32kHz freq - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); - // configure complementary pwm on low side - if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; - else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; - }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){ - // set prescaler to 1 - 32kHz freq - TCCR2B = ((TCCR2B & 0b11111000) | 0x01); - // configure complementary pwm on low side - if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; - else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; - }else{ - return -1; - } - return 0; -} - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -// supports Arudino/ATmega328 -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - // High PWM frequency - // - always max 32kHz - int ret_flag = 0; - ret_flag += _configureComplementaryPair(pinA_h, pinA_l); - ret_flag += _configureComplementaryPair(pinB_h, pinB_l); - ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured -} - -// function setting the -void _setPwmPair(int pinH, int pinL, float val, int dead_time) -{ - int pwm_h = _constrain(val-dead_time/2,0,255); - int pwm_l = _constrain(val+dead_time/2,0,255); - - analogWrite(pinH, pwm_h); - if(pwm_l == 255 || pwm_l == 0) - digitalWrite(pinL, pwm_l ? LOW : HIGH); - else - analogWrite(pinL, pwm_l); -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -// supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); -} - -#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp deleted file mode 100644 index 2c64ee64..00000000 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include "../hardware_api.h" - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) - -// set pwm frequency to 32KHz -void _pinHighFrequency(const int pin){ - // High PWM frequency - // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - } - if (pin == 9 || pin == 10 ) - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) - TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -// supports Arudino/ATmega328 -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - _UNUSED(pwm_frequency); - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); -} - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// supports Arudino/ATmega328 -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - _UNUSED(pwm_frequency); - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pinA); - _pinHighFrequency(pinB); - _pinHighFrequency(pinC); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); -} - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -// supports Arudino/ATmega328 -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - // High PWM frequency - // - always max 32kHz - _pinHighFrequency(pin1A); - _pinHighFrequency(pin1B); - _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); -} - - -// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm -int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ - // configure the pwm phase-corrected mode - TCCR0A = ((TCCR0A & 0b11111100) | 0x01); - // configure complementary pwm on low side - if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; - else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; - // set prescaler to 1 - 32kHz freq - TCCR0B = ((TCCR0B & 0b11110000) | 0x01); - }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ - // set prescaler to 1 - 32kHz freq - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); - // configure complementary pwm on low side - if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; - else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; - }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ - // set prescaler to 1 - 32kHz freq - TCCR2B = ((TCCR2B & 0b11111000) | 0x01); - // configure complementary pwm on low side - if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; - else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; - }else{ - return -1; - } - return 0; -} - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -// supports Arudino/ATmega328 -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); - // High PWM frequency - // - always max 32kHz - int ret_flag = 0; - ret_flag += _configureComplementaryPair(pinA_h, pinA_l); - ret_flag += _configureComplementaryPair(pinB_h, pinB_l); - ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured -} - -// function setting the -void _setPwmPair(int pinH, int pinL, float val, int dead_time) -{ - int pwm_h = _constrain(val-dead_time/2,0,255); - int pwm_l = _constrain(val+dead_time/2,0,255); - - analogWrite(pinH, pwm_h); - if(pwm_l == 255 || pwm_l == 0) - digitalWrite(pinL, pwm_l ? LOW : HIGH); - else - analogWrite(pinL, pwm_l); -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -// supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); -} - -#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index e0d39afd..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 @@ -320,13 +326,17 @@ void TC8_Handler() if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]); } + + + + // implementation of the hardware_api.cpp // --------------------------------------------------------------------------------------------------------------------------------- // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware specific -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -337,13 +347,21 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinC, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } + + // Configuring PWM frequency, resolution and alignment //- Stepper driver - 2PWM setting // - hardware specific -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -353,12 +371,21 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { initPWM(pinB, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -370,36 +397,52 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinD, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC, pinD); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); - setPwm(pinC, _max_pwm_value*dc_c); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); + setPwm(p->pins[2], _max_pwm_value*dc_c); } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pin1A, _max_pwm_value*dc_1a); - setPwm(pin1B, _max_pwm_value*dc_1b); - setPwm(pin2A, _max_pwm_value*dc_2a); - setPwm(pin2B, _max_pwm_value*dc_2b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_1a); + setPwm(p->pins[1], _max_pwm_value*dc_1b); + setPwm(p->pins[2], _max_pwm_value*dc_2a); + setPwm(p->pins[3], _max_pwm_value*dc_2b); } + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - Stepper driver - 2PWM setting // - hardware specific -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); } 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 new file mode 100644 index 00000000..5726e906 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -0,0 +1,158 @@ +#ifndef ESP32_DRIVER_MCPWM_H +#define ESP32_DRIVER_MCPWM_H + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "driver/mcpwm_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; \ + } + + +// 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 + + +// low-level configuration API + +/** + * 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 new file mode 100644 index 00000000..c5ba1c78 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -0,0 +1,405 @@ +#include "../../hardware_api.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 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 = 1024-1 + + +// figure out how many ledc channels are available +// esp32 - 2x8=16 +// esp32s2 - 8 +// esp32c3 - 6 +#include "soc/soc_caps.h" +#ifdef SOC_LEDC_SUPPORT_HS_MODE +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1) +#else +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) +#endif + +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) + + +// currently used ledc channels +// support for multiple motors +// esp32 has 16 channels +// esp32s2 has 8 channels +// esp32c3 has 6 channels +// 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 { + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; + long pwm_frequency; + float dead_zone; +} ESP32LEDCDriverParams; + + +/* + 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 + 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 = { 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; +} + + + + + + + + +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 + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); + + // 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 = { 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; +} + + + +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 + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); + + // 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 = { 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; +} + + + +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 + + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .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){ + _writeDutyCycle(dc_a, params, 0); +} + + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); +} + + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + _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){ + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); +} + + +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/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/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp deleted file mode 100644 index f6663d87..00000000 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ /dev/null @@ -1,435 +0,0 @@ -#include "../hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// ABI bus frequency - would be better to take it from somewhere -// but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6f - -// preferred pwm resolution default -#define _PWM_RES_DEF 2048 -// min resolution -#define _PWM_RES_MIN 1500 -// max resolution -#define _PWM_RES_MAX 3000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_motor_slots_t; -typedef struct { - int pin1A; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_1a; - mcpwm_io_signals_t mcpwm_1b; - mcpwm_io_signals_t mcpwm_2a; - mcpwm_io_signals_t mcpwm_2b; -} stepper_4pwm_motor_slots_t; -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; - -// define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A - {_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 -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.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].period.prescale = prescaler; - mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; - _delay(1); - //set period - mcpwm_num->timer[0].period.period = resolution_corrected; - mcpwm_num->timer[1].period.period = resolution_corrected; - mcpwm_num->timer[2].period.period = resolution_corrected; - _delay(1); - mcpwm_num->timer[0].period.upmethod = 0; - mcpwm_num->timer[1].period.upmethod = 0; - mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - // _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); - // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined - // in the default Espressif MCPWM headers. The correct const may be used - // when https://github.com/espressif/esp-idf/issues/5429 is resolved. - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, (mcpwm_sync_signal_t)1, 0); - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 1; - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 0; -} - -// 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); - -} - - -// 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); - -} - - -// 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); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - break; - } - } -} - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); - break; - } - } -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); - break; - } - } -} - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - - if(!pwm_frequency || !_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 -1; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_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 - return 0; -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - break; - } - } -} -#endif diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index ccc31aa2..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 @@ -15,63 +21,101 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _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 { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } -#endif \ No newline at end of file +#endif diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index f89c0562..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,31 +1,45 @@ + #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 +// function setting the high pwm frequency to the supplied pin +// - Stepper motor - 1PWM setting +// - hardware speciffic +// in generic case dont do anything +__attribute__((weak)) void* _configure1PWM(long pwm_frequency, const int pinA) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - _UNUSED(pwm_frequency); - _UNUSED(pinA); - _UNUSED(pinB); - return; +__attribute__((weak)) void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - _UNUSED(pwm_frequency); - _UNUSED(pinA); - _UNUSED(pinB); - _UNUSED(pinC); - return; +__attribute__((weak)) void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } @@ -33,75 +47,79 @@ __attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, con // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - _UNUSED(pwm_frequency); - _UNUSED(pin1A); - _UNUSED(pin1B); - _UNUSED(pin2A); - _UNUSED(pin2B); - return; +__attribute__((weak)) void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin1B, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +__attribute__((weak)) void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ _UNUSED(pwm_frequency); _UNUSED(dead_zone); _UNUSED(pinA_h); - _UNUSED(pinB_h); - _UNUSED(pinC_h); _UNUSED(pinA_l); + _UNUSED(pinB_h); _UNUSED(pinB_l); + _UNUSED(pinC_h); _UNUSED(pinC_l); - return -1; + + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 1PWM setting +// - hardware speciffic +__attribute__((weak)) 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); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ _UNUSED(dc_a); _UNUSED(dc_b); _UNUSED(dc_c); - _UNUSED(dead_zone); - _UNUSED(pinA_h); - _UNUSED(pinB_h); - _UNUSED(pinC_h); - _UNUSED(pinA_l); - _UNUSED(pinB_l); - _UNUSED(pinC_l); - return; -} + _UNUSED(phase_state); + _UNUSED(params); +} \ No newline at end of file diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp new file mode 100644 index 00000000..a20b828a --- /dev/null +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -0,0 +1,397 @@ + +#include "../hardware_api.h" + +#if defined(NRF52_SERIES) + +#pragma message("") +#pragma message("SimpleFOC: compiling for NRF52") +#pragma message("") + + +#define PWM_CLK (16000000) +#define PWM_FREQ (40000) +#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ) +#define PWM_MAX_FREQ (62500) +#define DEAD_ZONE (250) // in ns +#define DEAD_TIME (DEAD_ZONE / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM + +#ifdef NRF_PWM3 +#define PWM_COUNT 4 +#else +#define PWM_COUNT 3 +#endif + +// empty motor slot +#define _EMPTY_SLOT (-0xAA) +#define _TAKEN_SLOT (-0x55) + +int pwm_range; + + +static NRF_PWM_Type* pwms[PWM_COUNT] = { + NRF_PWM0, + NRF_PWM1, + NRF_PWM2, + #ifdef NRF_PWM3 + NRF_PWM3 + #endif +}; + +typedef struct { + int pinA; + NRF_PWM_Type* mcpwm; + uint16_t mcpwm_channel_sequence[4]; +} bldc_3pwm_motor_slots_t; + +typedef struct { + int pin1A; + NRF_PWM_Type* mcpwm; + uint16_t mcpwm_channel_sequence[4]; +} stepper_motor_slots_t; + +typedef struct { + int pinAH; + NRF_PWM_Type* mcpwm1; + NRF_PWM_Type* mcpwm2; + uint16_t mcpwm_channel_sequence[8]; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0 + {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1 + {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2 + {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3 + }; + +// define stepper motor slots array +stepper_motor_slots_t nrf52_stepper_motor_slots[4] = { + {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0 + {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1 + {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2 + {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3 + }; + +// define BLDC motor slots array +bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1 + {_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2 + }; + + + +typedef struct NRF52DriverParams { + union { + bldc_3pwm_motor_slots_t* slot3pwm; + bldc_6pwm_motor_slots_t* slot6pwm; + stepper_motor_slots_t* slotstep; + } slot; + long pwm_frequency; + float dead_time; +} NRF52DriverParams; + + + + +// configuring high frequency pwm timer +void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){ + + mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos); + mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos); + mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); + mcpwm1->COUNTERTOP = pwm_range; //pwm freq. + mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos); + mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos); + mcpwm1->SEQ[0].REFRESH = 0; + mcpwm1->SEQ[0].ENDDELAY = 0; + + if(mcpwm1 != mcpwm2){ + mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos); + mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos); + mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); + mcpwm2->COUNTERTOP = pwm_range; //pwm freq. + mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos); + mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos); + mcpwm2->SEQ[0].REFRESH = 0; + mcpwm2->SEQ[0].ENDDELAY = 0; + }else{ + mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos); + } +} + + + +// can we support it using the generic driver on this MCU? Commented out to fall back to generic driver for 2-pwm +// void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +// return SIMPLEFOC_DRIVER_INIT_FAILED; // not supported +// } + + + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 + else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + + int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA]; + int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB]; + int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC]; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + break; + } + } + // if no slots available + if(slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED; + + // disable all the slots with the same MCPWM + if(slot_num < 2){ + // slot 0 of the stepper + nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~1UL; + }else{ + // slot 1 of the stepper + nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~2UL; + } + + // configure pwm outputs + + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC; + + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4; + + // configure the pwm + _configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm); + + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slot3pwm = &(nrf52_bldc_3pwm_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + return params; +} + + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { + + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800 + else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + + int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA]; + int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB]; + int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC]; + int pD = digitalPinToPinName(pinD); //g_ADigitalPinMap[pinD]; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_stepper_motor_slots[slot_num].pin1A = pinA; + break; + } + } + // if no slots available + if (slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED; + + // disable all the slots with the same MCPWM + if (slot_num < 2){ + // slots 0 and 1 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~1UL; + }else{ + // slots 2 and 3 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + //NRF_PPI->CHEN &= ~2UL; + } + + // configure pwm outputs + + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC; + nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD; + + nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4; + + // configure the pwm + _configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm); + + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slotstep = &(nrf52_stepper_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + return params; +} + + + + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + // transform duty cycle from [0,1] to [0,range] + bldc_3pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot3pwm; + p->mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000; + + p->mcpwm->TASKS_SEQSTART[0] = 1; +} + + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + + stepper_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slotstep; + p->mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000; + p->mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000; + + p->mcpwm->TASKS_SEQSTART[0] = 1; +} + +/* Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +*/ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400 + else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256 + + pwm_range = (PWM_CLK / pwm_frequency); + pwm_range /= 2; // scale the frequency (centered PWM) + + float dead_time; + if (dead_zone != NOT_SET){ + dead_time = dead_zone/2; + }else{ + dead_time = DEAD_TIME/2; + } + + int pA_l = digitalPinToPinName(pinA_l); //g_ADigitalPinMap[pinA_l]; + int pA_h = digitalPinToPinName(pinA_h); //g_ADigitalPinMap[pinA_h]; + int pB_l = digitalPinToPinName(pinB_l); //g_ADigitalPinMap[pinB_l]; + int pB_h = digitalPinToPinName(pinB_h); //g_ADigitalPinMap[pinB_h]; + int pC_l = digitalPinToPinName(pinC_l); //g_ADigitalPinMap[pinC_l]; + int pC_h = digitalPinToPinName(pinC_h); //g_ADigitalPinMap[pinC_h]; + + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + break; + } + } + // if no slots available + if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 and 1 of the stepper + nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the stepper + nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT; + nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT; + } + + // Configure pwm outputs + + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0]; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4; + + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4]; + nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4; + + // Initializing the PPI peripheral for sync the pwm slots + + NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0]; + NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0]; + NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0]; + NRF_PPI->CHEN = 1UL << slot_num; + + // configure the pwm type + _configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2); + + NRF52DriverParams* params = new NRF52DriverParams(); + params->slot.slot6pwm = &(nrf52_bldc_6pwm_motor_slots[slot_num]); + params->pwm_frequency = pwm_frequency; + params->dead_time = dead_time; + return params; +} + + + + +/* Function setting the duty cycle to the pwm pin +// - BLDC driver - 6PWM setting +// - hardware specific +*/ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + bldc_6pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot6pwm; + float dead_time = ((NRF52DriverParams*)params)->dead_time; + p->mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range); + p->mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range); + p->mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000; + p->mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range); + NRF_EGU0->TASKS_TRIGGER[0] = 1; + + _UNUSED(phase_state); +} + + +#endif diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b314ee40..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" @@ -21,13 +27,14 @@ // #define _SOFTWARE_6PWM 0 // #define _ERROR_6PWM -1 -typedef struct{ - int id ; - pwmout_t pins[6]; -} portenta_h7_mcu_enty_s; -portenta_h7_mcu_enty_s motor_slot[10]; -int slot_index = 0; + +typedef struct PortentaDriverParams { + pwmout_t pins[4]; + long pwm_frequency; +// float dead_zone; +} PortentaDriverParams; + /* Convert STM32 Cube HAL channel to LL channel */ @@ -379,112 +386,111 @@ void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1])); core_util_critical_section_exit(); - slot_index++; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[3]), pinD, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2]), &(params->pins[3])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting //- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ -for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_c); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - for(int i=0; ipins[0]), (float)dc_1a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_1b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_2a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[3]), (float)dc_2b); + core_util_critical_section_exit(); } - +// 6-PWM currently not supported, defer to generic, which also doesn't support it ;-) // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +//void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ // if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz // else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // // center-aligned frequency is uses two periods @@ -510,13 +516,13 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // _alignPWMTimers(HT1, HT2, HT3); // break; // } - return -1; // success -} +// return -1; // success +// } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +//void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ // // find configuration // int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); // // set pwm accordingly @@ -535,5 +541,5 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); // break; // } -} +//} #endif \ No newline at end of file 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 new file mode 100644 index 00000000..6afbf345 --- /dev/null +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -0,0 +1,249 @@ + +/** + * 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 "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 1 + + + +// until I can figure out if this can be quickly read from some register, keep it here. +// it also serves as a marker for what slices are already used. +uint16_t wrapvalues[NUM_PWM_SLICES]; + + +// TODO add checks which channels are already used... + +void setupPWM(int pin_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; + 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((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((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) { + if (chan==0) + hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS); + else + hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS); + } + pwm_set_chan_level(slice, chan, 0); // switch off initially +} + + +void syncSlices() { + for (uint i=0;ipwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + syncSlices(); + return params; +} + + + +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + syncSlices(); + return params; +} + + + +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pinC, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); + syncSlices(); + return params; +} + + + + +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pin2A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); + setupPWM(pin2B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 3); + syncSlices(); + 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) { + // non-PIO solution... + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + params->dead_zone = dead_zone; + setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); + setupPWM(pinB_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 2); + setupPWM(pinC_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 4); + setupPWM(pinA_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 1); + setupPWM(pinB_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 3); + setupPWM(pinC_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 5); + syncSlices(); + return params; +} + + + + + +void writeDutyCycle(float val, uint slice, uint chan) { + pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val); +} + + + + +void _writeDutyCycle1PWM(float dc_a, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); +} + + + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); +} + + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); +} + + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { + writeDutyCycle(dc_1a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_1b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_2a, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + writeDutyCycle(dc_2b, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); +} + +inline float swDti(float val, float dt) { + float ret = dt+val; + if (ret>1.0) ret = 1.0f; + return ret; +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params) { + if (phase_state[0]==PhaseState::PHASE_ON || phase_state[0]==PhaseState::PHASE_HI) + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + if (phase_state[0]==PhaseState::PHASE_ON || phase_state[0]==PhaseState::PHASE_LO) + writeDutyCycle(swDti(dc_a, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + + if (phase_state[1]==PhaseState::PHASE_ON || phase_state[1]==PhaseState::PHASE_HI) + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + if (phase_state[1]==PhaseState::PHASE_ON || phase_state[1]==PhaseState::PHASE_LO) + writeDutyCycle(swDti(dc_b, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); + + if (phase_state[2]==PhaseState::PHASE_ON || phase_state[2]==PhaseState::PHASE_HI) + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]); + if (phase_state[2]==PhaseState::PHASE_ON || phase_state[2]==PhaseState::PHASE_LO) + writeDutyCycle(swDti(dc_c, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]); + else + writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]); + + _UNUSED(phase_state); +} + +#endif diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h new file mode 100644 index 00000000..bbfb3873 --- /dev/null +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -0,0 +1,22 @@ + + +#pragma once + +#include "Arduino.h" + +#if defined(TARGET_RP2040) + + + +typedef struct RP2040DriverParams { + int pins[6]; + uint slice[6]; + uint chan[6]; + long pwm_frequency; + float dead_zone; +} RP2040DriverParams; + + + + +#endif diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp deleted file mode 100644 index 7be8faaf..00000000 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ /dev/null @@ -1,163 +0,0 @@ - -/** - * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. - */ -#if defined(TARGET_RP2040) - -#define SIMPLEFOC_DEBUG_RP2040 - - -#ifdef SIMPLEFOC_DEBUG_RP2040 - -#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL -#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial -#endif - -#endif - -#include "Arduino.h" - - - - -// until I can figure out if this can be quickly read from some register, keep it here. -// it also serves as a marker for what slices are already used. -uint16_t wrapvalues[NUM_PWM_SLICES]; - - -// TODO add checks which channels are already used... - -void setupPWM(int pin, long pwm_frequency, bool invert = false) { - gpio_set_function(pin, GPIO_FUNC_PWM); - uint slice = pwm_gpio_to_slice_num(pin); - uint chan = pwm_gpio_to_channel(pin); - pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get - pwm_set_phase_correct(slice, true); - uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; - if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 - if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 -#ifdef SIMPLEFOC_DEBUG_RP2040 - SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue); -#endif - pwm_set_wrap(slice, wrapvalue); - wrapvalues[slice] = wrapvalue; - if (invert) { - if (chan==0) - hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS); - else - hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS); - } - pwm_set_chan_level(slice, chan, 0); // switch off initially -} - - -void syncSlices() { - for (int i=0;i1.0) ret = 1.0f; - return ret; -} - -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { - writeDutyCycle(dc_a, pinA_h); - writeDutyCycle(swDti(dc_a, dead_zone), pinA_l); - writeDutyCycle(dc_b, pinB_h); - writeDutyCycle(swDti(dc_b,dead_zone), pinB_l); - writeDutyCycle(dc_c, pinC_h); - writeDutyCycle(swDti(dc_c,dead_zone), pinC_l); -} - -#endif diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd/samd21_mcu.cpp similarity index 82% rename from src/drivers/hardware_specific/samd21_mcu.cpp rename to src/drivers/hardware_specific/samd/samd21_mcu.cpp index ab41b745..d59a3098 100644 --- a/src/drivers/hardware_specific/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 @@ -159,11 +163,12 @@ void configureSAMDClock() { REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC | // Set the duty cycle to 50/50 HIGH/LOW GCLK_GENCTRL_GENEN | // Enable GCLK4 GCLK_GENCTRL_SRC_DFLL48M | // Set the 48MHz clock source + // GCLK_GENCTRL_SRC_FDPLL | // Set the 96MHz clock source GCLK_GENCTRL_ID(4); // Select GCLK4 while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); + SIMPLEFOC_DEBUG("SAMD: Configured clock..."); #endif } } @@ -177,6 +182,15 @@ void configureSAMDClock() { * faster won't be possible without sacrificing resolution. */ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { + + long pwm_resolution = (24000000) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolutionCOUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: Initialized TC ", tccConfig.tcc.tccn); #endif } else { @@ -234,12 +247,12 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, if (hw6pwm>0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -254,14 +267,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } } @@ -279,22 +294,24 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, if (hw6pwm>0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } tcc->CTRLA.bit.ENABLE = 1; while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } @@ -305,18 +322,18 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CC // tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); // while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + //while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); // tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); @@ -324,7 +341,7 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { // while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { - Tc* tc = (Tc*)GetTC(chaninfo); + Tc* tc = (Tc*)GetTC(info->tcc.chaninfo); tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd/samd51_mcu.cpp similarity index 78% rename from src/drivers/hardware_specific/samd51_mcu.cpp rename to src/drivers/hardware_specific/samd/samd51_mcu.cpp index b59a43fc..71bf0b81 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd51_mcu.cpp @@ -6,6 +6,51 @@ #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. +#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ +#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000 +#endif + + +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER +#define TCC3_CH1 NOT_ON_TIMER +#endif + +#ifndef TCC4_CH0 +#define TCC4_CH0 NOT_ON_TIMER +#define TCC4_CH1 NOT_ON_TIMER +#endif + + +#ifndef TC4_CH0 +#define TC4_CH0 NOT_ON_TIMER +#define TC4_CH1 NOT_ON_TIMER +#endif + +#ifndef TC5_CH0 +#define TC5_CH0 NOT_ON_TIMER +#define TC5_CH1 NOT_ON_TIMER +#endif + +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#define TC6_CH1 NOT_ON_TIMER +#endif + +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#define TC7_CH1 NOT_ON_TIMER +#endif + + // TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation // 0 6 8 24-bit Yes Yes Yes Yes Yes Yes @@ -17,7 +62,6 @@ #define NUM_WO_ASSOCIATIONS 72 - struct wo_association WO_associations[] = { { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, @@ -51,7 +95,7 @@ struct wo_association WO_associations[] = { { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, - { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, + { PORTA, 15, TC3_CH1, 1, TCC2_CH1, 1, TCC1_CH3, 3}, { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, @@ -85,7 +129,7 @@ struct wo_association WO_associations[] = { { PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, { PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, { PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0}, - { PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0}, + { PORTB, 29, NOT_ON_TIMER, 0, TCC1_CH1, 5, NOT_ON_TIMER, 0}, // PC24-PC28, PA27, RESET -> no TC/TCC peripherals { PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, { PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, @@ -100,7 +144,12 @@ struct wo_association WO_associations[] = { wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; +#ifndef TCC3_CC_NUM +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM }; +#else uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; +#endif + struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { for (int i=0;i>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; + return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT; } @@ -124,24 +173,17 @@ void syncTCC(Tcc* TCCx) { -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CCBUF - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! -// tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); -// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); -// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency! } - else { - // Tc* tc = (Tc*)GetTC(chaninfo); - // //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - // tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); - // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + else { + // we don't support the TC channels on SAMD51, isn't worth it. } } @@ -183,12 +225,12 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC - | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); - //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); + //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; syncTCC(tcc); // wait for sync + // work out pwm resolution for desired frequency and constrain to max/min values + long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolution0.0) { tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); - tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } @@ -232,7 +283,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this? while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -247,18 +298,20 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { - Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); + //Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); // disable // tc->COUNT8.CTRLA.bit.ENABLE = 0; @@ -280,8 +333,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: Not initialized: TC ", tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: TC units not supported on SAMD51"); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd/samd_mcu.cpp similarity index 71% rename from src/drivers/hardware_specific/samd_mcu.cpp rename to src/drivers/hardware_specific/samd/samd_mcu.cpp index e1d38a80..f6978914 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd_mcu.cpp @@ -278,7 +278,7 @@ int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguratio * @param pinA pinA bldc driver * @param pinB pinB bldc driver */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -287,9 +287,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)), @@ -297,9 +297,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 2)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,21 +306,32 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif - return; // Someone with a stepper-setup who can test it? + SAMDHardwareDriverParams* params = new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; // Someone with a stepper-setup who can test it? + return params; } @@ -368,7 +377,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { * @param pinB pinB bldc driver * @param pinC pinC bldc driver */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -377,9 +386,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)), @@ -388,9 +397,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 3)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,21 +408,34 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB), getTccPinConfiguration(pinC) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; + } @@ -436,7 +456,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in * @param pin2A pin2A stepper driver * @param pin2B pin2B stepper driver */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); #endif @@ -445,9 +465,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif - return; + return SIMPLEFOC_DRIVER_INIT_FAILED; } tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, getPeripheralOfPermutation(compatibility, 0)), @@ -457,9 +477,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 4)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,23 +490,35 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); + getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res; + getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif - return; // Someone with a stepper-setup who can test it? + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pin1A), getTccPinConfiguration(pin1B), getTccPinConfiguration(pin2A), getTccPinConfiguration(pin2B) }, + .pwm_frequency = (uint32_t)pwm_frequency + }; } @@ -528,7 +558,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const * * @return 0 if config good, -1 if failed */ -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion #ifdef SIMPLEFOC_SAMD_DEBUG printAllPinInfos(); @@ -539,9 +569,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif - return -1; + return SIMPLEFOC_DRIVER_INIT_FAILED; } } @@ -553,7 +583,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); + SIMPLEFOC_DEBUG("SAMD: Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -571,14 +601,19 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const allAttached |= attachTCC(pinCh); allAttached |= attachTCC(pinCl); if (!allAttached) - return -1; + return SIMPLEFOC_DRIVER_INIT_FAILED; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC(s) configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1); if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo)) @@ -589,11 +624,21 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1); if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); + getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res; + getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it + getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res; + getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif - return 0; + return new SAMDHardwareDriverParams { + .tccPinConfigurations = { getTccPinConfiguration(pinA_h), getTccPinConfiguration(pinA_l), getTccPinConfiguration(pinB_h), getTccPinConfiguration(pinB_l), getTccPinConfiguration(pinC_h), getTccPinConfiguration(pinC_l) }, + .pwm_frequency = (uint32_t)pwm_frequency, + .dead_zone = dead_zone, + }; } @@ -609,11 +654,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const * @param pinA phase A hardware pin number * @param pinB phase B hardware pin number */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b); return; } @@ -633,13 +676,10 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { * @param pinB phase B hardware pin number * @param pinC phase C hardware pin number */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); - tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_c); return; } @@ -660,15 +700,11 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB * @param pin2A phase 2A hardware pin number * @param pin2B phase 2B hardware pin number */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - tccConfiguration* tccI = getTccPinConfiguration(pin1A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); - tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); - tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); - tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_1a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_1b); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_2a); + writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[3], dc_2b); return; } @@ -698,41 +734,45 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in * @param pinC_l phase C low-side hardware pin number * */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); - tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + SAMDHardwareDriverParams* p = (SAMDHardwareDriverParams*)params; + tccConfiguration* tcc1 = p->tccPinConfigurations[0]; + tccConfiguration* tcc2 = p->tccPinConfigurations[1]; + uint32_t pwm_res =p->tccPinConfigurations[0]->pwm_res; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... - float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_a+(p->dead_zone * (pwm_res-1)); // TODO resolution!!! if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_a); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly + writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly - tcc1 = getTccPinConfiguration(pinB_h); - tcc2 = getTccPinConfiguration(pinB_l); + tcc1 = p->tccPinConfigurations[2]; + tcc2 = p->tccPinConfigurations[3]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_b+(p->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_b); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tcc1, dc_b); - tcc1 = getTccPinConfiguration(pinC_h); - tcc2 = getTccPinConfiguration(pinC_l); + tcc1 = p->tccPinConfigurations[4]; + tcc2 = p->tccPinConfigurations[5]; if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + float ls = dc_c+(p->dead_zone * (pwm_res-1)); if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_c); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tcc1, dc_c); return; + + _UNUSED(phase_state); } @@ -746,85 +786,91 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); + SimpleFOCDebug::println(); for (uint8_t pin=0;pin=TCC_INST_NUM){ + SimpleFOCDebug::print(" TC"); + SimpleFOCDebug::print(tcn-TCC_INST_NUM); + } + else { + SimpleFOCDebug::print("TCC"); + SimpleFOCDebug::print(tcn); + } + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(GetTCChannelNumber(association.tccE)); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(GetTCChannelNumber(association.woE)); + SimpleFOCDebug::print("]"); if (tcn<10) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" "); + SimpleFOCDebug::print(" "); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" None "); + SimpleFOCDebug::print(" None "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F="); + SimpleFOCDebug::print(" F="); if (association.tccF>=0) { int tcn = GetTCNumber(association.tccF); - if (tcn>=TCC_INST_NUM) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" TC"); - else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccF)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woF)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]"); + if (tcn>=TCC_INST_NUM){ + SimpleFOCDebug::print(" TC"); + SimpleFOCDebug::print(tcn-TCC_INST_NUM); + } + else { + SimpleFOCDebug::print("TCC"); + SimpleFOCDebug::print(tcn); + } + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(GetTCChannelNumber(association.tccF)); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(GetTCChannelNumber(association.woF)); + SimpleFOCDebug::print("]"); if (tcn<10) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" "); + SimpleFOCDebug::print(" "); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" None "); + SimpleFOCDebug::print(" None "); #if defined(_SAMD51_)||defined(_SAME51_) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G="); + SimpleFOCDebug::print(" G="); if (association.tccG>=0) { int tcn = GetTCNumber(association.tccG); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC"); + SimpleFOCDebug::print("TCC"); if (tcn<10) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccG)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woG)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SimpleFOCDebug::print(" "); + SimpleFOCDebug::print(tcn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(GetTCChannelNumber(association.tccG)); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(GetTCChannelNumber(association.woG)); + SimpleFOCDebug::println("]"); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None "); + SimpleFOCDebug::println(" None "); #else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(""); + SimpleFOCDebug::println(); #endif } - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); + SimpleFOCDebug::println(); } @@ -832,33 +878,33 @@ void printAllPinInfos() { void printTCCConfiguration(tccConfiguration& info) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.pin); + SimpleFOCDebug::print(info.pin); if (info.tcc.tccn>=TCC_INST_NUM) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); + SimpleFOCDebug::print(": TC Peripheral"); else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); + SimpleFOCDebug::print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; + SimpleFOCDebug::print(" E "); break; case PIO_TIMER_ALT: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; + SimpleFOCDebug::print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; + SimpleFOCDebug::print(" G "); break; #endif default: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; + SimpleFOCDebug::print(" ? "); break; } if (info.tcc.tccn>=0) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SimpleFOCDebug::print(info.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(info.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(info.wo); + SimpleFOCDebug::println("]"); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); + SimpleFOCDebug::println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd/samd_mcu.h similarity index 69% rename from src/drivers/hardware_specific/samd_mcu.h rename to src/drivers/hardware_specific/samd/samd_mcu.h index 7faf7442..74004d65 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd/samd_mcu.h @@ -3,13 +3,11 @@ #define SAMD_MCU_H -// uncomment to enable debug output to Serial port +// uncomment to enable debug output from SAMD driver +// can set this as build-flag in Arduino IDE or PlatformIO // #define SIMPLEFOC_SAMD_DEBUG -#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) -#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial -#endif -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__SAME51J19A__) || defined(__ATSAME51J19A__) @@ -29,9 +27,17 @@ #ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION #define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 -#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 #endif +#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000 +// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency +#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000 +// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency... +// 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency... +#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400 +// this is the most we can support on the TC units +#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 + #ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS #define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 #endif @@ -49,6 +55,7 @@ struct tccConfiguration { }; uint16_t chaninfo; } tcc; + uint16_t pwm_res; }; @@ -71,6 +78,15 @@ struct wo_association { +typedef struct SAMDHardwareDriverParams { + tccConfiguration* tccPinConfigurations[6]; + uint32_t pwm_frequency; + float dead_zone; +} SAMDHardwareDriverParams; + + + + #if defined(_SAMD21_) #define NUM_PIO_TIMER_PERIPHERALS 2 #elif defined(_SAMD51_)||defined(_SAME51_) @@ -92,7 +108,7 @@ extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; struct wo_association& getWOAssociation(EPortType port, uint32_t pin); -void writeSAMDDutyCycle(int chaninfo, float dc); +void writeSAMDDutyCycle(tccConfiguration* info, float dc); void configureSAMDClock(); void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1); __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp new file mode 100644 index 00000000..4009281e --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -0,0 +1,1190 @@ + +#include "../../hardware_api.h" +#include "stm32_mcu.h" + +#if defined(_STM32_DEF_) + +#define SIMPLEFOC_STM32_DEBUG +#pragma message("") +#pragma message("SimpleFOC: compiling for STM32") +#pragma message("") + + +#ifdef SIMPLEFOC_STM32_DEBUG +void printTimerCombination(int numPins, PinMap* timers[], int score); +int getTimerNumber(int timerIndex); +#endif + + + + +#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED +#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12 +#endif +int numTimerPinsUsed; +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() +void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution) +{ + // TODO - remove commented code + // PinName pin = digitalPinToPinName(ulPin); + // TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + // uint32_t index = get_timer_index(Instance); + // HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + + + +int getLLChannel(PinMap* timer) { +#if defined(TIM_CCER_CC1NE) + if (STM_PIN_INVERTED(timer->function)) { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1N; + case 2: return LL_TIM_CHANNEL_CH2N; + case 3: return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case 4: return LL_TIM_CHANNEL_CH4N; +#endif + default: return -1; + } + } else +#endif + { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1; + case 2: return LL_TIM_CHANNEL_CH2; + case 3: return LL_TIM_CHANNEL_CH3; + case 4: return LL_TIM_CHANNEL_CH4; + default: return -1; + } + } + return -1; +} + + + + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { + // sanity check + if (timer==NP) + return NP; + uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); + bool init = false; + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + init = true; + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(timer->function); + HT->pause(); + if (init) + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin); + #if SIMPLEFOC_PWM_ACTIVE_HIGH==false + LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); + #endif +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Configuring high timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); + SIMPLEFOC_DEBUG("STM32-DRV: Configuring high channel ", (int)channel); +#endif + return HT; +} + + + + + + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { + HardwareTimer* HT = _initPinPWM(PWM_freq, timer); + #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false && SIMPLEFOC_PWM_ACTIVE_HIGH==true + LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); + #endif + return HT; +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer) +{ + uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); + + bool init = false; + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + init = true; + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(timer->function); + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Configuring low timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); + SIMPLEFOC_DEBUG("STM32-DRV: Configuring low channel ", (int)channel); +#endif + + HT->pause(); + if (init) + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + // sets internal fields of HT, but we can't set polarity here + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin); + #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false + LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); + #endif + return HT; +} + + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + + + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + + +// align the timers to end the init +void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) +{ + // TODO - stop each timer only once + // stop timers + for (int i=0; i < timer_num; i++) { + if(timers_to_stop[i] == NP) return; + timers_to_stop[i]->pause(); + timers_to_stop[i]->refresh(); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", getTimerNumber(get_timer_index(timers_to_stop[i]->getHandle()->Instance))); + #endif + } +} + + +#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(); + } +} + +void _alignTimersNew() { + int numTimers = 0; + HardwareTimer *timers[numTimerPinsUsed]; + + // find the timers used + for (int i=0; iperipheral); + HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + bool found = false; + for (int j=0; j 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(); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); + #endif + } + + for (int i=0; iresume(); + } + +} + + + +// 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 +STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { + // sanity check + if (pinH==NP || pinL==NP) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + +#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface + return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing +#endif + + uint32_t channel1 = STM_PIN_CHANNEL(pinH->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinL->function); + + // more sanity check + if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; + // HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin); + HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin); + + // 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); + #endif + #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false + LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinL), LL_TIM_OCPOLARITY_LOW); + #endif + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); + HT->pause(); + + // make sure timer output goes to LOW when timer channels are temporarily disabled + LL_TIM_SetOffStates(HT->getHandle()->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); + + params->timers[paramsPos] = HT; + params->timers[paramsPos+1] = HT; + params->channels[paramsPos] = channel1; + params->channels[paramsPos+1] = channel2; + return params; +} + + + + +STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { + STM32DriverParams* params = new STM32DriverParams { + .timers = { NP, NP, NP, NP, NP, NP }, + .channels = { 0, 0, 0, 0, 0, 0 }, + .pwm_frequency = PWM_freq, + .dead_zone = dead_zone, + .interface_type = _HARDWARE_6PWM + }; + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if(_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + return params; +} + + + + + + +/* + timer combination scoring function + assigns a score, and also checks the combination is valid + returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better + for 6 pwm, hardware 6-pwm is preferred over software 6-pwm + hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel + inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) +*/ +int scoreCombination(int numPins, PinMap* pinTimers[]) { + // check already used - TODO move this to outer loop also... + for (int i=0; iperipheral == timerPinsUsed[i]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function)) + return -2; // bad combination - timer channel already used + } + + // TODO LPTIM and HRTIM should be ignored for now + + // check for inverted channels + if (numPins < 6) { + for (int i=0; ifunction)) + return -3; // bad combination - inverted channel used in non-hardware 6pwm + } + } + // check for duplicated channels + for (int i=0; iperipheral == pinTimers[j]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) + && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) + return -4; // bad combination - duplicated channel + } + } + int score = 0; + for (int i=0; iperipheral == pinTimers[j]->peripheral) + found = true; + } + if (!found) score++; + } + if (numPins==6) { + // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels + // >1 timer, 3 channels, 3 matching inverted channels + // 1 timer, 6 channels (no inverted channels) + // >1 timer, 6 channels (no inverted channels) + // check for inverted high-side channels - TODO is this a configuration we should allow? what if all 3 high side channels are inverted and the low-side non-inverted? + if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + return -5; // bad combination - inverted channel used on high-side channel + if (pinTimers[0]->peripheral == pinTimers[1]->peripheral + && pinTimers[2]->peripheral == pinTimers[3]->peripheral + && pinTimers[4]->peripheral == pinTimers[5]->peripheral + && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) + && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) + && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) + && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { + // hardware 6pwm, score <10 + + // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 + // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion + // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion + // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1 + + // TODO check these defines + #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H) + if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 ) + return -8; // channel 4 does not have dead-time insertion + #endif + #ifdef STM32G4xx_HAL_TIM_H + if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 ) + return -8; // channels 5 & 6 do not have dead-time insertion + #endif + } + else { + // check for inverted low-side channels + if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) + return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm + if (pinTimers[0]->peripheral != pinTimers[1]->peripheral + || pinTimers[2]->peripheral != pinTimers[3]->peripheral + || pinTimers[4]->peripheral != pinTimers[5]->peripheral) + return -7; // bad combination - non-matching timers for H/L side in software 6-pwm + score += 10; // software 6pwm, score >10 + } + } + return score; +} + + + + +int findIndexOfFirstPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if (pinName == PinMap_TIM[i].pin) + return i; + i++; + } + return -1; +} + + +int findIndexOfLastPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) + && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} + + + + + + +#define NOT_FOUND 1000 + +int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { + PinMap* searchArray[numPins]; + for (int j=0;j=0 && score= 0) { + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: best: "); + printTimerCombination(numPins, pinTimers, bestScore); + #endif + } + return bestScore; +}; + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + if (numTimerPinsUsed+1 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + int pins[1] = { pinA }; + PinMap* pinTimers[1] = { NP }; + if (findBestTimerCombination(1, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ + // align the timers + _alignTimersNew(); + + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1 }, + .channels = { channel1 }, + .pwm_frequency = pwm_frequency + }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + return params; +} + + + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if (numTimerPinsUsed+2 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + int pins[2] = { pinA, pinB }; + PinMap* pinTimers[2] = { NP, NP }; + if (findBestTimerCombination(2, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + 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); + + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2 }, + .channels = { channel1, channel2 }, + .pwm_frequency = pwm_frequency + }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + return params; +} + + + + +TIM_MasterConfigTypeDef sMasterConfig; +TIM_SlaveConfigTypeDef sSlaveConfig; + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if (numTimerPinsUsed+3 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + int pins[3] = { pinA, pinB, pinC }; + PinMap* pinTimers[3] = { NP, NP, NP }; + if (findBestTimerCombination(3, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + 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); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2, HT3 }, + .channels = { channel1, channel2, channel3 }, + .pwm_frequency = pwm_frequency + }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; + + _alignTimersNew(); + + 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 (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + int pins[4] = { pinA, pinB, pinC, pinD }; + PinMap* pinTimers[4] = { NP, NP, NP, NP }; + if (findBestTimerCombination(4, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); + 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); + + 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); + uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); + + STM32DriverParams* params = new STM32DriverParams { + .timers = { HT1, HT2, HT3, HT4 }, + .channels = { channel1, channel2, channel3, channel4 }, + .pwm_frequency = pwm_frequency + }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[3]; + return params; +} + + + +// function setting the pwm duty cycle to the hardware +// - DC motor - 1PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// 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 (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + // find configuration + int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }; + PinMap* pinTimers[6] = { NP, NP, NP, NP, NP, NP }; + int score = findBestTimerCombination(6, pins, pinTimers); + + STM32DriverParams* params; + // configure accordingly + if (score<0) + params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + else if (score<10) // hardware pwm + params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); + else { // software pwm + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); + 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); + uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); + uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); + uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); + params = new STM32DriverParams { + .timers = { HT1, HT2, HT3, HT4, HT5, HT6 }, + .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone, + .interface_type = _SOFTWARE_6PWM + }; + } + if (score>=0) { + for (int i=0; i<6; i++) + timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; + _alignTimersNew(); + } + return params; // success +} + + + +void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) { + _UNUSED(channel2); + switch (state) { + case PhaseState::PHASE_OFF: + // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE). + // To actually make the phase floating, we must also set pwm to 0. + HT->pauseChannel(channel1); + break; + default: + HT->resumeChannel(channel1); + break; + } +} + + +// 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){ + switch(((STM32DriverParams*)params)->interface_type){ + case _HARDWARE_6PWM: + // phase a + _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]); + if(phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + // phase b + _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], ((STM32DriverParams*)params)->channels[3]); + if(phase_state[1] == PhaseState::PHASE_OFF) dc_b = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + // phase c + _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], ((STM32DriverParams*)params)->channels[5]); + if(phase_state[2] == PhaseState::PHASE_OFF) dc_c = 0.0f; + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + float dead_zone = ((STM32DriverParams*)params)->dead_zone / 2.0f; + if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_HI) + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], 0.0f, _PWM_RESOLUTION); + if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_LO) + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], 0.0f, _PWM_RESOLUTION); + + if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_HI) + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], 0.0f, _PWM_RESOLUTION); + if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_LO) + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], 0.0f, _PWM_RESOLUTION); + + if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_HI) + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], 0.0f, _PWM_RESOLUTION); + if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_LO) + _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + else + _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], 0.0f, _PWM_RESOLUTION); + break; + } + _UNUSED(phase_state); +} + + + +#ifdef SIMPLEFOC_STM32_DEBUG + +int getTimerNumber(int timerIndex) { + #if defined(TIM1_BASE) + if (timerIndex==TIMER1_INDEX) return 1; + #endif + #if defined(TIM2_BASE) + if (timerIndex==TIMER2_INDEX) return 2; + #endif + #if defined(TIM3_BASE) + if (timerIndex==TIMER3_INDEX) return 3; + #endif + #if defined(TIM4_BASE) + if (timerIndex==TIMER4_INDEX) return 4; + #endif + #if defined(TIM5_BASE) + if (timerIndex==TIMER5_INDEX) return 5; + #endif + #if defined(TIM6_BASE) + if (timerIndex==TIMER6_INDEX) return 6; + #endif + #if defined(TIM7_BASE) + if (timerIndex==TIMER7_INDEX) return 7; + #endif + #if defined(TIM8_BASE) + if (timerIndex==TIMER8_INDEX) return 8; + #endif + #if defined(TIM9_BASE) + if (timerIndex==TIMER9_INDEX) return 9; + #endif + #if defined(TIM10_BASE) + if (timerIndex==TIMER10_INDEX) return 10; + #endif + #if defined(TIM11_BASE) + if (timerIndex==TIMER11_INDEX) return 11; + #endif + #if defined(TIM12_BASE) + if (timerIndex==TIMER12_INDEX) return 12; + #endif + #if defined(TIM13_BASE) + if (timerIndex==TIMER13_INDEX) return 13; + #endif + #if defined(TIM14_BASE) + if (timerIndex==TIMER14_INDEX) return 14; + #endif + #if defined(TIM15_BASE) + if (timerIndex==TIMER15_INDEX) return 15; + #endif + #if defined(TIM16_BASE) + if (timerIndex==TIMER16_INDEX) return 16; + #endif + #if defined(TIM17_BASE) + if (timerIndex==TIMER17_INDEX) return 17; + #endif + #if defined(TIM18_BASE) + if (timerIndex==TIMER18_INDEX) return 18; + #endif + #if defined(TIM19_BASE) + if (timerIndex==TIMER19_INDEX) return 19; + #endif + #if defined(TIM20_BASE) + if (timerIndex==TIMER20_INDEX) return 20; + #endif + #if defined(TIM21_BASE) + if (timerIndex==TIMER21_INDEX) return 21; + #endif + #if defined(TIM22_BASE) + if (timerIndex==TIMER22_INDEX) return 22; + #endif + return -1; +} + + +void printTimerCombination(int numPins, PinMap* timers[], int score) { + for (int i=0; iperipheral))); + SimpleFOCDebug::print("-CH"); + SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); + if (STM_PIN_INVERTED(timers[i]->function)) + SimpleFOCDebug::print("N"); + } + SimpleFOCDebug::print(" "); + } + SimpleFOCDebug::println("score: ", score); +} + +#endif + +#endif diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h new file mode 100644 index 00000000..411c43b2 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -0,0 +1,35 @@ +#ifndef STM32_DRIVER_MCU_DEF +#define STM32_DRIVER_MCU_DEF +#include "../../hardware_api.h" + +#if defined(_STM32_DEF_) + +// default pwm parameters +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0f // 2^12 -1 = 4095 +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +// 6pwm parameters +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +typedef struct STM32DriverParams { + HardwareTimer* timers[6] = {NULL}; + uint32_t channels[6]; + long pwm_frequency; + float dead_zone; + uint8_t interface_type; +} STM32DriverParams; + +// timer synchornisation functions +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/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp deleted file mode 100644 index 540dd6c2..00000000 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ /dev/null @@ -1,355 +0,0 @@ - -#include "../hardware_api.h" - -#if defined(_STM32_DEF_) -// default pwm parameters -#define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095 -#define _PWM_FREQUENCY 25000 // 25khz -#define _PWM_FREQUENCY_MAX 50000 // 50khz - -// 6pwm parameters -#define _HARDWARE_6PWM 1 -#define _SOFTWARE_6PWM 0 -#define _ERROR_6PWM -1 - - -// setting pwm to hardware pin - instead analogWrite() -void _setPwm(int ulPin, uint32_t value, int resolution) -{ - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); -} - - -// init pin pwm -HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) -{ - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - - uint32_t index = get_timer_index(Instance); - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - HT->pause(); - HT->refresh(); - return HT; -} - - -// init high side pin -HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) -{ - return _initPinPWM(PWM_freq, ulPin); -} - -// init low side pin -HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) -{ - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); - - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); - sConfigOC.OCMode = TIM_OCMODE_PWM2; - sConfigOC.Pulse = 100; - sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; -#if defined(TIM_OCIDLESTATE_SET) - sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; -#endif -#if defined(TIM_OCNIDLESTATE_RESET) - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; -#endif - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - HT->pause(); - HT->refresh(); - return HT; -} - - -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); -} - -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT4->pause(); - HT4->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); - HT4->resume(); -} - -// configure hardware 6pwm interface only one timer with inverted channels -HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) -{ - -#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface - PinName uhPinName = digitalPinToPinName(pinA_h); - PinName ulPinName = digitalPinToPinName(pinA_l); - PinName vhPinName = digitalPinToPinName(pinB_h); - PinName vlPinName = digitalPinToPinName(pinB_l); - PinName whPinName = digitalPinToPinName(pinC_h); - PinName wlPinName = digitalPinToPinName(pinC_l); - - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - - uint32_t index = get_timer_index(Instance); - - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); - HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); - - // dead time is set in nanoseconds - uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; - uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); - LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! - LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); - - // Set Trigger out for DMA transfer - LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - - HT->pause(); - HT->refresh(); - - // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs. - HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - HT->getHandle()->Instance->CNT = TIM1->ARR; - - HT->resume(); - return HT; -#else - return nullptr; // return nothing -#endif -} - - -// returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels -// returns -1 if neither - avoid configuring - error!!! -int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - PinName nameAH = digitalPinToPinName(pinA_h); - PinName nameAL = digitalPinToPinName(pinA_l); - PinName nameBH = digitalPinToPinName(pinB_h); - PinName nameBL = digitalPinToPinName(pinB_l); - PinName nameCH = digitalPinToPinName(pinC_h); - PinName nameCL = digitalPinToPinName(pinC_l); - int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); - int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); - int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); - int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); - int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); - int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); - -#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface - - if(tim1 == tim2 && tim3==tim4 && tim5==tim6) - return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer - else - return _ERROR_6PWM; // might be error avoid configuration -#else // the rest of stm32 boards - - if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer - else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) - return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer - else - return _ERROR_6PWM; // might be error avoid configuration -#endif -} - - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; - - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - // allign the timers - _alignPWMTimers(HT1, HT2, HT2); -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; - - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - // allign the timers - _alignPWMTimers(HT1, HT2, HT3); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; - - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); - // allign the timers - _alignPWMTimers(HT1, HT2, HT3, HT4); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -//- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); -} - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -//- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); -} - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -//- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); - _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); - _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); - _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); -} - - - - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; - - // find configuration - int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - // configure accordingly - switch(config){ - case _ERROR_6PWM: - return -1; // fail - break; - case _HARDWARE_6PWM: - _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - break; - case _SOFTWARE_6PWM: - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); - _initPinPWMLow(pwm_frequency, pinA_l); - HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); - _initPinPWMLow(pwm_frequency, pinB_l); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); - _initPinPWMLow(pwm_frequency, pinC_l); - _alignPWMTimers(HT1, HT2, HT3); - break; - } - return 0; // success -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // find configuration - int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - // set pwm accordingly - switch(config){ - case _HARDWARE_6PWM: - _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); - break; - case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - break; - } -} -#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp new file mode 100644 index 00000000..fe145273 --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -0,0 +1,227 @@ +#include "teensy_mcu.h" + +// if defined +// - Teensy 3.0 MK20DX128 +// - Teensy 3.1/3.2 MK20DX256 +// - 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__)) + + +#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 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM0_CH6_PIN 21 +#define FTM0_CH7_PIN 5 +#define FTM1_CH0_PIN 3 +#define FTM1_CH1_PIN 4 +#elif defined(__MK20DX256__) +#define FTM0_CH0_PIN 22 +#define FTM0_CH1_PIN 23 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM0_CH6_PIN 21 +#define FTM0_CH7_PIN 5 +#define FTM1_CH0_PIN 3 +#define FTM1_CH1_PIN 4 +#define FTM2_CH0_PIN 32 +#define FTM2_CH1_PIN 25 +#elif defined(__MKL26Z64__) +#define FTM0_CH0_PIN 22 +#define FTM0_CH1_PIN 23 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM1_CH0_PIN 16 +#define FTM1_CH1_PIN 17 +#define FTM2_CH0_PIN 3 +#define FTM2_CH1_PIN 4 +#elif defined(__MK64FX512__) +#define FTM0_CH0_PIN 22 +#define FTM0_CH1_PIN 23 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM0_CH6_PIN 21 +#define FTM0_CH7_PIN 5 +#define FTM1_CH0_PIN 3 +#define FTM1_CH1_PIN 4 +#define FTM2_CH0_PIN 29 +#define FTM2_CH1_PIN 30 +#define FTM3_CH0_PIN 2 +#define FTM3_CH1_PIN 14 +#define FTM3_CH2_PIN 7 +#define FTM3_CH3_PIN 8 +#define FTM3_CH4_PIN 35 +#define FTM3_CH5_PIN 36 +#define FTM3_CH6_PIN 37 +#define FTM3_CH7_PIN 38 +#elif defined(__MK66FX1M0__) +#define FTM0_CH0_PIN 22 +#define FTM0_CH1_PIN 23 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM0_CH6_PIN 21 +#define FTM0_CH7_PIN 5 +#define FTM1_CH0_PIN 3 +#define FTM1_CH1_PIN 4 +#define FTM2_CH0_PIN 29 +#define FTM2_CH1_PIN 30 +#define FTM3_CH0_PIN 2 +#define FTM3_CH1_PIN 14 +#define FTM3_CH2_PIN 7 +#define FTM3_CH3_PIN 8 +#define FTM3_CH4_PIN 35 +#define FTM3_CH5_PIN 36 +#define FTM3_CH6_PIN 37 +#define FTM3_CH7_PIN 38 +#define TPM1_CH0_PIN 16 +#define TPM1_CH1_PIN 17 +#endif + +int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const int Ch, const int Cl){ + + if((Ah == FTM0_CH0_PIN && Al == FTM0_CH1_PIN) || + (Ah == FTM0_CH2_PIN && Al == FTM0_CH3_PIN) || + (Ah == FTM0_CH4_PIN && Al == FTM0_CH5_PIN) ){ + if((Bh == FTM0_CH0_PIN && Bl == FTM0_CH1_PIN) || + (Bh == FTM0_CH2_PIN && Bl == FTM0_CH3_PIN) || + (Bh == FTM0_CH4_PIN && Bl == FTM0_CH5_PIN) ){ + if((Ch == FTM0_CH0_PIN && Cl == FTM0_CH1_PIN) || + (Ch == FTM0_CH2_PIN && Cl == FTM0_CH3_PIN) || + (Ch == FTM0_CH4_PIN && Cl == FTM0_CH5_PIN) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Using timer FTM0."); +#endif + // timer FTM0 + return 0; + } + } + } + +#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) ){ + if((Bh == FTM3_CH0_PIN && Bl == FTM3_CH1_PIN) || + (Bh == FTM3_CH2_PIN && Bl == FTM3_CH3_PIN) || + (Bh == FTM3_CH4_PIN && Bl == FTM3_CH5_PIN) ){ + if((Ch == FTM3_CH0_PIN && Cl == FTM3_CH1_PIN) || + (Ch == FTM3_CH2_PIN && Cl == FTM3_CH3_PIN) || + (Ch == FTM3_CH4_PIN && Cl == FTM3_CH5_PIN) ){ + // timer FTM3 +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Using timer FTM3."); +#endif + return 3; + } + } + } +#endif + +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Pins not on timers FTM0 or FTM3!"); +#endif + return -1; + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 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 = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + unsigned long pwm_freq = 2*pwm_frequency; // center-aligned pwm has 4 times lower freq + _setHighFrequency(pwm_freq, pinA_h); + _setHighFrequency(pwm_freq, pinA_l); + _setHighFrequency(pwm_freq, pinB_h); + _setHighFrequency(pwm_freq, pinB_l); + _setHighFrequency(pwm_freq, pinC_h); + _setHighFrequency(pwm_freq, pinC_l); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,pinA_l, pinB_h,pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + }; + + + int timer = _findTimer(pinA_h,pinA_l,pinB_h,pinB_l,pinC_h,pinC_l); + if(timer<0) return SIMPLEFOC_DRIVER_INIT_FAILED; + + // find the best combination of prescalers and counter value + double dead_time = dead_zone/pwm_freq; + int prescaler = 1; // initial prescaler (1,4 or 16) + double count = 1; // inital count (1 - 63) + for (; prescaler<=16; prescaler*=4){ + count = dead_time*((double)F_CPU)/((double)prescaler); + if(count < 64) break; // found the solution + } + count = _constrain(count, 1, 63); + + // configure the timer + if(timer==0){ + // Configure FTM0 + // // inverting and deadtime insertion for FTM1 + FTM0_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled + + // Deadtime config + FTM0_DEADTIME = (int)count; // set counter - 1-63 + FTM0_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16) + + // configure center aligned PWM + FTM0_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq + }else if(timer==3){ + // Configure FTM3 + // inverting and deadtime insertion for FTM1 + FTM3_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled + + // Deadtime config + FTM3_DEADTIME = (int)count; // set counter - 1-63 + FTM3_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16) + + // configure center aligned PWM + FTM3_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq + } + + return params; +} + + + +// 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, 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); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_a); + + // phase B + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_b); + + // phase C + analogWrite(((GenericDriverParams*)params)->pins[4], 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[5], 255.0f*dc_c); +} +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..47108447 --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,666 @@ +#include "teensy4_mcu.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) ) + +#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){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; + switch ((info->module >> 4) & 3) { + case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; + default: flexpwm1 = &IMXRT_FLEXPWM4; + } + + info = pwm_pin_info + pin1; + switch ((info->module >> 4) & 3) { + case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; + default: flexpwm2 = &IMXRT_FLEXPWM4; + } + if(flexpwm1 == flexpwm2){ + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); + return flexpwm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } +} + + +// 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_submodule(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]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + info = pwm_pin_info + pin1; + int sm2 = info->module&0x3; + + if (sm1 == sm2) { + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); + SIMPLEFOC_DEBUG(s); + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + + +// 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){ + + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); + + if (ch1 != 1) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else if (ch2 != 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); + SIMPLEFOC_DEBUG(s); +#endif +return ch2; + } +} + +// Helper to set up A/B pair on a FlexPWM submodule. +// 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, 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 + 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); + 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(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 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) + 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 = -mid_pwm ; + flexpwm->SM[submodule].VAL3 = +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) +{ + int submodule_mask = 1 << submodule ; + + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + +// 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->MCTRL |= FLEXPWM_MCTRL_LDOK (1<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); +} + +#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 new file mode 100644 index 00000000..aed64826 --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,125 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + +#include "teensy_mcu.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) ) + +// teensy 4 driver configuration parameters +typedef struct Teensy4DriverParams { + IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; + int submodules[3]; + int channels[6]; + float dead_zone; +} Teensy4DriverParams; + + +// pin definition from https://github.com/PaulStoffregen/cores/blob/master/teensy4/pwm.c +struct pwm_pin_info_struct { + uint8_t type; // 0=no pwm, 1=flexpwm, 2=quad + uint8_t module; // 0-3, 0-3 + uint8_t channel; // 0=X, 1=A, 2=B + uint8_t muxval; // +}; +#define M(a, b) ((((a) - 1) << 4) | (b)) +#if defined(__IMXRT1062__) +const struct pwm_pin_info_struct pwm_pin_info[] = { + {1, M(1, 1), 0, 4}, // FlexPWM1_1_X 0 // AD_B0_03 + {1, M(1, 0), 0, 4}, // FlexPWM1_0_X 1 // AD_B0_02 + {1, M(4, 2), 1, 1}, // FlexPWM4_2_A 2 // EMC_04 + {1, M(4, 2), 2, 1}, // FlexPWM4_2_B 3 // EMC_05 + {1, M(2, 0), 1, 1}, // FlexPWM2_0_A 4 // EMC_06 + {1, M(2, 1), 1, 1}, // FlexPWM2_1_A 5 // EMC_08 + {1, M(2, 2), 1, 2}, // FlexPWM2_2_A 6 // B0_10 + {1, M(1, 3), 2, 6}, // FlexPWM1_3_B 7 // B1_01 + {1, M(1, 3), 1, 6}, // FlexPWM1_3_A 8 // B1_00 + {1, M(2, 2), 2, 2}, // FlexPWM2_2_B 9 // B0_11 + {2, M(1, 0), 0, 1}, // QuadTimer1_0 10 // B0_00 + {2, M(1, 2), 0, 1}, // QuadTimer1_2 11 // B0_02 + {2, M(1, 1), 0, 1}, // QuadTimer1_1 12 // B0_01 + {2, M(2, 0), 0, 1}, // QuadTimer2_0 13 // B0_03 + {2, M(3, 2), 0, 1}, // QuadTimer3_2 14 // AD_B1_02 + {2, M(3, 3), 0, 1}, // QuadTimer3_3 15 // AD_B1_03 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {2, M(3, 1), 0, 1}, // QuadTimer3_1 18 // AD_B1_01 + {2, M(3, 0), 0, 1}, // QuadTimer3_0 19 // AD_B1_00 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(4, 0), 1, 1}, // FlexPWM4_0_A 22 // AD_B1_08 + {1, M(4, 1), 1, 1}, // FlexPWM4_1_A 23 // AD_B1_09 + {1, M(1, 2), 0, 4}, // FlexPWM1_2_X 24 // AD_B0_12 + {1, M(1, 3), 0, 4}, // FlexPWM1_3_X 25 // AD_B0_13 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(3, 1), 2, 1}, // FlexPWM3_1_B 28 // EMC_32 + {1, M(3, 1), 1, 1}, // FlexPWM3_1_A 29 // EMC_31 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(2, 0), 2, 1}, // FlexPWM2_0_B 33 // EMC_07 +#ifdef ARDUINO_TEENSY40 + {1, M(1, 1), 2, 1}, // FlexPWM1_1_B 34 // SD_B0_03 + {1, M(1, 1), 1, 1}, // FlexPWM1_1_A 35 // SD_B0_02 + {1, M(1, 0), 2, 1}, // FlexPWM1_0_B 36 // SD_B0_01 + {1, M(1, 0), 1, 1}, // FlexPWM1_0_A 37 // SD_B0_00 + {1, M(1, 2), 2, 1}, // FlexPWM1_2_B 38 // SD_B0_05 + {1, M(1, 2), 1, 1}, // FlexPWM1_2_A 39 // SD_B0_04 +#endif +#ifdef ARDUINO_TEENSY41 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(2, 3), 1, 6}, // FlexPWM2_3_A 36 // B1_00 + {1, M(2, 3), 2, 6}, // FlexPWM2_3_B 37 // B1_01 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(1, 1), 2, 1}, // FlexPWM1_1_B 42 // SD_B0_03 + {1, M(1, 1), 1, 1}, // FlexPWM1_1_A 43 // SD_B0_02 + {1, M(1, 0), 2, 1}, // FlexPWM1_0_B 44 // SD_B0_01 + {1, M(1, 0), 1, 1}, // FlexPWM1_0_A 45 // SD_B0_00 + {1, M(1, 2), 2, 1}, // FlexPWM1_2_B 46 // SD_B0_05 + {1, M(1, 2), 1, 1}, // FlexPWM1_2_A 47 // SD_B0_04 + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_0_B + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_2_A + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_2_B + {1, M(3, 3), 2, 1}, // FlexPWM3_3_B 51 // EMC_22 + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_1_B + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_1_A + {1, M(3, 0), 1, 1}, // FlexPWM3_0_A 54 // EMC_29 +#endif +#ifdef ARDUINO_TEENSY_MICROMOD + {1, M(1, 1), 2, 1}, // FlexPWM1_1_B 34 // SD_B0_03 + {1, M(1, 1), 1, 1}, // FlexPWM1_1_A 35 // SD_B0_02 + {1, M(1, 0), 2, 1}, // FlexPWM1_0_B 36 // SD_B0_01 + {1, M(1, 0), 1, 1}, // FlexPWM1_0_A 37 // SD_B0_00 + {1, M(1, 2), 1, 1}, // FlexPWM1_2_A 38 // SD_B0_04 + {1, M(1, 2), 2, 1}, // FlexPWM1_2_B 39 // SD_B0_05 + {2, M(2, 1), 0, 1}, // QuadTimer2_1 40 // B0_04 + {2, M(2, 2), 0, 1}, // QuadTimer2_2 41 // B0_05 + {0, M(1, 0), 0, 0}, // duplicate QuadTimer3_0 + {0, M(1, 0), 0, 0}, // duplicate QuadTimer3_1 + {0, M(1, 0), 0, 0}, // duplicate QuadTimer3_2 + {2, M(4, 0), 0, 1}, // QuadTimer4_0 45 // B0_09 +#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 new file mode 100644 index 00000000..196f07fd --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -0,0 +1,151 @@ +#include "teensy_mcu.h" + +#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); + analogWriteFrequency(pin, freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - 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); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; + return params; +} + + +// 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 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB }, + .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 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 + + // 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 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 + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; + return params; +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware specific +void _writeDutyCycle1PWM(float dc_a, void* params) { + // transform duty cycle from [0,1] to [0,255] + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*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) { + // transform duty cycle from [0,1] to [0,255] + 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 specific +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + + 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 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(((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 new file mode 100644 index 00000000..266f4b69 --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -0,0 +1,27 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + +#include "../../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +// 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/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp deleted file mode 100644 index 67bb0ad1..00000000 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include "../hardware_api.h" - -#if defined(__arm__) && defined(CORE_TEENSY) - -#define _PWM_FREQUENCY 25000 // 25khz -#define _PWM_FREQUENCY_MAX 50000 // 50khz - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin){ - analogWrite(pin, 0); - analogWriteFrequency(pin, freq); -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); -} - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - _setHighFrequency(pwm_frequency, pinD); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); -} -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); -} - -#endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 7c164878..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; } @@ -205,6 +204,7 @@ void Encoder::init(){ // change it if the mode is quadrature if(quadrature == Quadrature::ON) cpr = 4*cpr; + // we don't call Sensor::init() here because init is handled in Encoder class. } // function enabling hardware interrupts of the for the callback provided diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 3e775338..af6a5ab6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -10,9 +10,9 @@ /** * Quadrature mode configuration structure */ -enum Quadrature{ - ON, //!< Enable quadrature mode CPR = 4xPPR - OFF //!< Disable quadrature mode / CPR = PPR +enum Quadrature : uint8_t { + ON = 0x00, //!< Enable quadrature mode CPR = 4xPPR + OFF = 0x01 //!< Disable quadrature mode / CPR = PPR }; class Encoder: public Sensor{ @@ -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/GenericSensor.cpp b/src/sensors/GenericSensor.cpp new file mode 100644 index 00000000..3d4025f3 --- /dev/null +++ b/src/sensors/GenericSensor.cpp @@ -0,0 +1,26 @@ +#include "GenericSensor.h" + + +/* + GenericSensor( float (*readCallback)() ) + - readCallback - pointer to the function which reads the sensor angle. +*/ + +GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){ + // if function provided add it to the + if(readCallback != nullptr) this->readCallback = readCallback; + if(initCallback != nullptr) this->initCallback = initCallback; +} + +void GenericSensor::init(){ + // if init callback specified run it + if(initCallback != nullptr) this->initCallback(); + this->Sensor::init(); // call base class init +} + +/* + Shaft angle calculation +*/ +float GenericSensor::getSensorAngle(){ + return this->readCallback(); +} \ No newline at end of file diff --git a/src/sensors/GenericSensor.h b/src/sensors/GenericSensor.h new file mode 100644 index 00000000..ba0dfe5c --- /dev/null +++ b/src/sensors/GenericSensor.h @@ -0,0 +1,31 @@ +#ifndef GENERIC_SENSOR_LIB_H +#define GENERIC_SENSOR_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" + + +class GenericSensor: public Sensor{ + public: + /** + GenericSensor class constructor + * @param readCallback pointer to the function reading the sensor angle + * @param initCallback pointer to the function initialising the sensor + */ + GenericSensor(float (*readCallback)() = nullptr, void (*initCallback)() = nullptr); + + float (*readCallback)() = nullptr; //!< function pointer to sensor reading + void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation + + void init() override; + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getSensorAngle() override; + +}; + + +#endif diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 467ba9e7..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,34 +127,18 @@ float HallSensor::getMechanicalAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + noInterrupts(); + long last_pulse_timestamp = pulse_timestamp; + long last_pulse_diff = pulse_diff; + 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 { - return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + 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(){ @@ -159,13 +157,14 @@ void HallSensor::init(){ } // init hall_state - A_active= digitalRead(pinA); + A_active = digitalRead(pinA); B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); pulse_timestamp = _micros(); + // we don't call Sensor::init() here because init is handled in HallSensor class. } // function enabling hardware interrupts for the callback provided @@ -177,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 9e80ed2b..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); @@ -76,6 +76,9 @@ class HallSensor: public Sensor{ // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) volatile long total_interrupts; + // variable used to filter outliers - rad/s + float velocity_max = 1000.0f; + private: Direction decodeDirection(int oldState, int newState); diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 6d881657..d4adad60 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -24,6 +24,8 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ raw_count = getRawCount(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 6c61b8ce..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,12 +67,18 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ wire = &Wire; } +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; // I2C communication begin wire->begin(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation @@ -93,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); @@ -105,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 5088fa9e..04b7cc75 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -10,7 +10,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m pinPWM = _pinPWM; - cpr = _max_raw_count - _min_raw_count; + cpr = _max_raw_count - _min_raw_count + 1; min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; @@ -22,25 +22,70 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m } +/** MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks) + * + * Constructor that computes the min and max raw counts based on the PWM frequency and the number of PWM clocks in one period + * + * @param _pinPWM the pin that is reading the pwm from magnetic sensor + * @param freqHz the frequency of the PWM signal, in Hz, e.g. 115, 230, 460 or 920 for the AS5600, depending on the PWM frequency setting + * @param _total_pwm_clocks the total number of PWM clocks in one period, e.g. 4351 for the AS5600 + * @param _min_pwm_clocks the 0 value returned by the sensor, in PWM clocks, e.g. 128 for the AS5600 + * @param _max_pwm_clocks the largest value returned by the sensor, in PWM clocks, e.g. 4223 for the AS5600 + */ +MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks){ + + pinPWM = _pinPWM; + + min_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_min_pwm_clocks); + max_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_max_pwm_clocks); + cpr = max_raw_count - min_raw_count + 1; + + // define if the sensor uses interrupts + is_interrupt_based = false; + + min_elapsed_time = 1.0f/freqHz; // set the minimum time between two readings + + // define as not set + last_call_us = _micros(); +} + + + 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 raw_count = getRawCount(); - return( (float) (raw_count) / (float)cpr) * _2PI; + if (raw_count > max_raw_count) raw_count = max_raw_count; + if (raw_count < min_raw_count) raw_count = min_raw_count; + return( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI; } // 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; } @@ -51,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 b42b23d6..a5fd7e6e 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -10,17 +10,31 @@ class MagneticSensorPWM: public Sensor{ public: - /** - * MagneticSensorPWM class constructor - * @param _pinPWM the pin to read the PWM sensor input signal + /** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max) + * @param _pinPWM the pin that is reading the pwm from magnetic sensor + * @param _min_raw_count the smallest expected reading + * @param _max_raw_count the largest expected reading */ MagneticSensorPWM(uint8_t _pinPWM,int _min = 0, int _max = 0); - + /** MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks) + * + * Constructor that computes the min and max raw counts based on the PWM frequency and the number of PWM clocks in one period + * + * @param _pinPWM the pin that is reading the pwm from magnetic sensor + * @param freqHz the frequency of the PWM signal, in Hz, e.g. 115, 230, 460 or 920 for the AS5600, depending on the PWM frequency setting + * @param _total_pwm_clocks the total number of PWM clocks in one period, e.g. 4351 for the AS5600 + * @param _min_pwm_clocks the 0 value returned by the sensor, in PWM clocks, e.g. 128 for the AS5600 + * @param _max_pwm_clocks the largest value returned by the sensor, in PWM clocks, e.g. 4223 for the AS5600 + */ + MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks); // initialize the sensor hardware void init(); int pinPWM; + + // Interrupt-safe update + void update() override; // get current angle (rad) float getSensorAngle() override; @@ -30,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; @@ -51,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 b7a9dd27..52febc38 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -1,4 +1,3 @@ -#ifndef TARGET_RP2040 #include "MagneticSensorSPI.h" @@ -32,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; @@ -53,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; @@ -74,6 +73,8 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // do any architectures need to set the clock divider for SPI? Why was this in the code? //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); + + this->Sensor::init(); // call base class init } // Shaft angle calculation @@ -158,4 +159,3 @@ void MagneticSensorSPI::close(){ } -#endif diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 33aad3f3..03ebde44 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -1,7 +1,6 @@ #ifndef MAGNETICSENSORSPI_LIB_H #define MAGNETICSENSORSPI_LIB_H -#ifndef TARGET_RP2040 #include "Arduino.h" #include @@ -31,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 @@ -83,4 +82,3 @@ class MagneticSensorSPI: public Sensor{ #endif -#endif