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/ccpp.yml b/.github/workflows/ccpp.yml index 6171ab77..b4830d82 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -12,16 +12,19 @@ jobs: - arduino:sam:arduino_due_x # arduino due - arduino:samd:nano_33_iot # samd21 - adafruit:samd:adafruit_metro_m4 # samd51 - - esp32:esp32:esp32doit-devkit-v1 # esm32 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - esp32:esp32:esp32doit-devkit-v1 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: 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 + 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, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino @@ -36,17 +39,29 @@ jobs: platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json sketch-names: single_full_control_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://dl.espressif.com/dl/package_esp32_index.json + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + - 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 + + - 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 - - 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 + - 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, sdouble_full_control_example.ino 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..25c728a5 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,7 @@ ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. @@ -12,38 +13,54 @@ Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop a modular FOC supporting BLDC driver boards: + - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini). - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). + - *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

+

NEW RELEASE 📢: SimpleFOClibrary v2.2.2 see release

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

@@ -53,58 +70,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, MEGA, DUE, Leonardo .... + - 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 +129,19 @@ 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 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). + +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 -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. +Please do not hesitate to leave an issue if you have problems/advices/suggestions regarding the code! -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. +Pull requests are welcome, but let's first discuss them in [community forum](https://community.simplefoc.com)! + +If you'd like to contribute to this porject but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. ## Arduino code example This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder. @@ -208,9 +215,6 @@ 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/) - ## Arduino FOC repo structure Branch | Description | Status 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..b6d6b8eb 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,12 @@ // 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); +LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, 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,11 +19,9 @@ 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() { @@ -40,6 +38,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(); @@ -94,14 +94,11 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // 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!!!! diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 202aa337..84033b42 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -79,6 +79,10 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage motor.voltage_sensor_align = 0.5; // control loop type and torque mode @@ -124,7 +128,6 @@ void setup() { motor.init(); cs.init(); - cs.driverSync(&driver); // driver 8302 has inverted gains on all channels cs.gain_a *=-1; cs.gain_b *=-1; diff --git a/examples/hardware_specific_examples/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..b89c215f --- /dev/null +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -0,0 +1,134 @@ +/* + 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(){ + + // 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; + + + Serial.begin(115200); + // 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..2aed6bfb --- /dev/null +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -0,0 +1,132 @@ +/* + 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(){ + + // 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; + + + Serial.begin(115200); + // 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/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index dfad6e3a..a508bb19 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -33,6 +33,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); motor.voltage_sensor_align = 1; // set control loop type to be used diff --git a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino new file mode 100644 index 00000000..9102c89a --- /dev/null +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -0,0 +1,125 @@ +/** + * + * 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() { + // 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; + + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // add target command 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/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 259b23ee..2d28790c 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -56,11 +56,16 @@ void setup() { driver1.init(); // link driver motor1.linkDriver(&driver1); + // link current sense and the driver + current_sense1.linkDriver(&driver1); + // power supply voltage [V] driver2.voltage_power_supply = 12; driver2.init(); // link driver motor2.linkDriver(&driver2); + // link current sense and the driver + current_sense2.linkDriver(&driver2); // set control loop type to be used motor1.controller = MotionControlType::torque; diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 2d6c3ca6..688c8b6a 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -17,7 +17,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +void doMotion(char* cmd){ command.motion(&motor, cmd); } // void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { @@ -34,6 +34,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // set control loop type to be used motor.controller = MotionControlType::torque; @@ -74,7 +76,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/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..83b1fb32 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -18,18 +18,29 @@ 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() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; driver.init(); // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp motor.voltage_limit = 3; // [V] + // limit/set the velocity of the transition in between + // target angles motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::angle_openloop; @@ -39,6 +50,8 @@ void setup() { // add target command T command.add('T', doTarget, "target angle"); + command.add('L', doLimit, "voltage limit"); + command.add('V', doLimit, "movement velocity"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index f7a82eb3..7a4a14ac 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -19,19 +19,26 @@ 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() { // driver config // power supply voltage [V] driver.voltage_power_supply = 12; + // limit the maximal dc voltage the driver can set + // as a protection measure for the low-resistance motors + // this value is fixed on startup + driver.voltage_limit = 6; driver.init(); // link the motor and the driver motor.linkDriver(&driver); // limiting motor movements + // limit the voltage to be set to the motor + // start very low for high resistance motors + // currnet = resistance*voltage, so try to be well under 1Amp motor.voltage_limit = 3; // [V] - motor.velocity_limit = 5; // [rad/s] cca 50rpm // open loop control config motor.controller = MotionControlType::velocity_openloop; @@ -41,6 +48,7 @@ void setup() { // add target command T command.add('T', doTarget, "target velocity"); + command.add('L', doLimit, "voltage limit"); Serial.begin(115200); Serial.println("Motor ready!"); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index 942f73dc..ccb3980e 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; @@ -57,8 +51,6 @@ void setup() { // 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 +63,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; diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index 8b1ec96b..3f3ccb18 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -39,6 +39,8 @@ void setup() { driver.init(); // link driver motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); // current sense init hardware current_sense.init(); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 3fcd7b30..7324edf0 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -35,9 +35,10 @@ void testAlignmentAndCogging(int direction) { for (int i = 0; i < sample_count; i++) { - float electricAngle = (float) direction * i * motor.pole_pairs * shaft_rotation / sample_count; + float shaftAngle = (float) direction * i * shaft_rotation / sample_count; + float electricAngle = (float) shaftAngle * motor.pole_pairs; // move and wait - motor.move(electricAngle * PI / 180); + motor.move(shaftAngle * PI / 180); _delay(5); // measure 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..76fb46b0 --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,102 @@ +/** + * + * 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*30.0f/_PI); + +} + +void setup() { + + // 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; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // 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..3abef467 --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,99 @@ +/** + * + * 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*30.0f/_PI); + +} + +void setup() { + + // 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; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // 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..f3dd74a1 --- /dev/null +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -0,0 +1,96 @@ +/** + * 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*30.0f/_PI); + +} + +void setup() { + + // 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; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // 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..aac879cd 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 @@ -56,7 +56,7 @@ void setup() { 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 +73,7 @@ void setup() { while(motor_angle <= pp_search_angle){ motor_angle += 0.01f; motor.move(motor_angle); + _delay(1); } _delay(1000); // read the encoder value for 180 @@ -89,9 +90,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..e98a4529 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 @@ -55,7 +55,7 @@ void setup() { 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 +73,7 @@ void setup() { motor_angle += 0.01f; sensor.update(); // keep track of the overflow motor.move(motor_angle); + _delay(1); } _delay(1000); // read the sensor value for 180 @@ -89,9 +90,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/current_sense_test/generic_current_sense/generic_current_sense.ino b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino new file mode 100644 index 00000000..e999de23 --- /dev/null +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -0,0 +1,59 @@ +/** + * An example code for the generic current sensing implementation +*/ +#include + + +// user defined function for reading the phase currents +// returning the value per phase in amps +PhaseCurrent_s readCurrentSense(){ + PhaseCurrent_s c; + // dummy example only reading analog pins + c.a = analogRead(A0); + c.b = analogRead(A1); + c.c = analogRead(A2); // if no 3rd current sense set it to 0 + return(c); +} + +// user defined function for intialising the current sense +// it is optional and if provided it will be called in current_sense.init() +void initCurrentSense(){ + pinMode(A0,INPUT); + pinMode(A1,INPUT); + pinMode(A2,INPUT); +} + + +// GenericCurrentSense class constructor +// it receives the user defined callback for reading the current sense +// and optionally the user defined callback for current sense initialisation +GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense); + + +void setup() { + // if callbacks are not provided in the constructor + // they can be assigned directly: + //current_sense.readCallback = readCurrentSense; + //current_sense.initCallback = initCurrentSense; + + // initialise the current sensing + current_sense.init(); + + + Serial.begin(115200); + Serial.println("Current sense ready."); +} + +void loop() { + + PhaseCurrent_s currents = current_sense.getPhaseCurrents(); + float current_magnitude = current_sense.getDCCurrent(); + + Serial.print(currents.a); // milli Amps + Serial.print("\t"); + Serial.print(currents.b); // milli Amps + Serial.print("\t"); + Serial.print(currents.c); // milli Amps + Serial.print("\t"); + Serial.println(current_magnitude); // milli Amps +} \ No newline at end of file diff --git a/examples/utils/sensor_test/generic_sensor/generic_sensor.ino b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino new file mode 100644 index 00000000..4a470e59 --- /dev/null +++ b/examples/utils/sensor_test/generic_sensor/generic_sensor.ino @@ -0,0 +1,51 @@ +/** + * Generic sensor example code + * + * This is a code intended to demonstrate how to implement the generic sensor class + * + */ + +#include + +// sensor reading function example +// for the magnetic sensor with analog communication +// returning an angle in radians in between 0 and 2PI +float readSensor(){ + return analogRead(A0)*_2PI/1024.0; +} + +// sensor intialising function +void initSensor(){ + pinMode(A0,INPUT); +} + +// generic sensor class contructor +// - read sensor callback +// - init sensor callback (optional) +GenericSensor sensor = GenericSensor(readSensor, initSensor); + +void setup() { + // monitoring port + Serial.begin(115200); + + // if callbacks are not provided in the constructor + // they can be assigned directly: + //sensor.readCallback = readSensor; + //sensor.initCallback = initSensor; + + sensor.init(); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} \ No newline at end of file diff --git a/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..e6f4e605 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 @@ -230,6 +235,7 @@ _PI_3 LITERAL1 _SQRT3 LITERAL1 _PI LITERAL1 _HIGH_Z LITERAL1 +_NC LITERAL1 _MON_TARGET LITERAL1 _MON_VOLT_Q LITERAL1 diff --git a/library.properties b/library.properties index 89540636..d4b639d5 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.2 +version=2.2.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index ff451e07..c417e684 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,15 +1,20 @@ #include "BLDCMotor.h" +#include "./communication/SimpleFOCDebug.h" // 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) +BLDCMotor::BLDCMotor(int pp, float _R, float _KV) : FOCMotor() { // save pole pairs number pole_pairs = pp; // save phase resistance number phase_resistance = _R; + // save back emf constant KV = 1/KV + KV_rating = _KV; + // torque control type is voltage by default torque_controller = TorqueControlType::voltage; } @@ -24,14 +29,14 @@ 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; + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return; } + 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,20 +47,22 @@ 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; _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; } @@ -86,6 +93,9 @@ void BLDCMotor::enable() // FOC initialization function int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + // align motor if necessary // alignment necessary for encoders! if(_isset(zero_electric_offset)){ @@ -103,7 +113,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else + SIMPLEFOC_DEBUG("MOT: No sensor."); // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases @@ -111,13 +122,15 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction _delay(500); if(exit_flag){ if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); + else SIMPLEFOC_DEBUG("MOT: No current sense."); } if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); + 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 +141,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); 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 +160,22 @@ 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; // 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; // 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); + sensor->update(); _delay(2); } // take and angle in the middle @@ -171,6 +185,7 @@ int BLDCMotor::alignSensor() { for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); _delay(2); } sensor->update(); @@ -179,24 +194,23 @@ int BLDCMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -212,13 +226,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 +240,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 +260,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(); } @@ -297,7 +310,7 @@ void BLDCMotor::loopFOC() { break; default: // no torque control selected - if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); break; } @@ -331,12 +344,19 @@ void BLDCMotor::move(float new_target) { if(!enabled) return; // set internal target variable if(_isset(new_target)) target = new_target; + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + // 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; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); voltage.d = 0; }else{ current_sp = target; // if current/foc_current torque control @@ -356,7 +376,7 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*phase_resistance; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); voltage.d = 0; } break; @@ -369,7 +389,7 @@ void BLDCMotor::move(float new_target) { if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = current_sp*phase_resistance; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); voltage.d = 0; } break; @@ -608,7 +628,8 @@ 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 + voltage_bemf,-voltage_limit, voltage_limit); // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -643,10 +664,10 @@ 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 + voltage_bemf,-voltage_limit, voltage_limit); // 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..65bacb8d 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -19,8 +19,9 @@ class BLDCMotor: public FOCMotor BLDCMotor class constructor @param pp pole pairs number @param R motor phase resistance + @param KV motor KV rating (1/K_bemf) - rpm/V */ - BLDCMotor(int pp, float R = NOT_SET); + BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET); /** * Function linking a motor and a foc driver diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4f9f1108..606046d0 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -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..f8b112ef 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,15 +1,20 @@ #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) +StepperMotor::StepperMotor(int pp, float _R, float _KV) : FOCMotor() { // number od pole pairs pole_pairs = pp; // save phase resistance number phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + KV_rating = _KV; // torque control type is voltage by default // current and foc_current not supported yet @@ -25,14 +30,14 @@ 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; + if (!driver || !driver->initialized) { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return; } + 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 @@ -49,10 +54,11 @@ void StepperMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); + motor_status = FOCMotorStatus::motor_uncalibrated; } @@ -84,6 +90,9 @@ void StepperMotor::enable() // FOC initialization function int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + // align motor if necessary // alignment necessary for encoders! if(_isset(zero_electric_offset)){ @@ -101,12 +110,14 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else SIMPLEFOC_DEBUG("MOT: No sensor."); if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); + 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(); } @@ -116,7 +127,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction if(!_isset(sensor_direction)){ @@ -147,24 +158,23 @@ int StepperMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -179,13 +189,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 +202,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 +222,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(); } @@ -272,11 +281,18 @@ void StepperMotor::move(float new_target) { // set internal target variable if(_isset(new_target) ) target = new_target; + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + // estimate the motor current if phase reistance available and current_sense not available + if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; + // choose control loop switch (controller) { case MotionControlType::torque: if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); voltage.d = 0; break; case MotionControlType::angle: @@ -289,8 +305,8 @@ void StepperMotor::move(float new_target) { // 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; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + voltage.d = 0; break; case MotionControlType::velocity: // velocity set point @@ -300,7 +316,7 @@ void StepperMotor::move(float new_target) { // 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; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); voltage.d = 0; break; case MotionControlType::velocity_openloop: @@ -361,7 +377,8 @@ 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 + voltage_bemf,-voltage_limit, voltage_limit); // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -395,7 +412,8 @@ 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 + voltage_bemf,-voltage_limit, voltage_limit); // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); diff --git a/src/StepperMotor.h b/src/StepperMotor.h index d0cd3e18..9ee39b79 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -24,8 +24,9 @@ class StepperMotor: public FOCMotor StepperMotor class constructor @param pp pole pair number @param R motor phase resistance + @param KV motor KV rating (1/K_bemf) - rpm/V */ - StepperMotor(int pp, float R = NOT_SET); + StepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET); /** * Function linking a motor and a foc driver diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index a657d17c..d40929ad 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -22,6 +22,9 @@ class BLDCDriver{ float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC + bool initialized = false; // true if driver was successfully initialized + void* params = 0; // pointer to hardware specific parameters of driver + /** * Set phase voltages to the harware * diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 2cec8dd5..becfa499 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -16,6 +16,16 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; + }if(!current.a){ + // if only two measured currents + float a = -current.c - current.b; + i_alpha = a; + i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; + }if(!current.b){ + // if only two measured currents + float b = -current.a - current.c; + i_alpha = current.a; + i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; }else{ // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. float mid = (1.f/3) * (current.a + current.b + current.c); @@ -48,6 +58,16 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; + }if(!current.a){ + // if only two measured currents + float a = -current.c - current.b; + i_alpha = a; + i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; + }if(!current.b){ + // if only two measured currents + float b = -current.a - current.c; + i_alpha = current.a; + i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; } else { // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. float mid = (1.f/3) * (current.a + current.b + current.c); @@ -64,4 +84,11 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ return_current.d = i_alpha * ct + i_beta * st; return_current.q = i_beta * ct - i_alpha * st; return return_current; +} + +/** + Driver linking to the current sense +*/ +void CurrentSense::linkDriver(BLDCDriver* _driver) { + driver = _driver; } \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 0904dea2..ad1bd9ca 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -13,19 +13,24 @@ class CurrentSense{ /** * Function intialising the CurrentSense class - * All the necessary intialisations of adc and sync should be implemented here + * - All the necessary intialisations of adc and sync should be implemented here + * + * @returns - 0 - for failure & 1 - for success */ - virtual void init() = 0; + virtual int init() = 0; /** - * Function intended to implement all that is needed to sync and current sensing with the driver. - * If no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & 1 - for success + * Linking the current sense with the motor driver + * Only necessary if synchronisation in between the two is required */ - virtual int driverSync(BLDCDriver *driver) = 0; + void linkDriver(BLDCDriver *driver); - // calibration variables + // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() + + BLDCDriver* driver; //!< driver link + void* params = 0; //!< pointer to hardware specific parameters of current sensing + /** * Function intended to verify if: * - phase current are oriented properly @@ -34,7 +39,7 @@ class CurrentSense{ * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) * @returns - 0 - for failure & positive number (with status) - for success */ - virtual int driverAlign(BLDCDriver *driver, float voltage) = 0; + virtual int driverAlign(float align_voltage) = 0; /** * Function rading the phase currents a, b and c @@ -62,6 +67,8 @@ class CurrentSense{ * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); + + }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 922076a6..dc0fc97e 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,6 +29,9 @@ FOCMotor::FOCMotor() current_sp = 0; current.q = 0; current.d = 0; + + // voltage bemf + voltage_bemf = 0; //monitor_port monitor_port = nullptr; @@ -77,7 +81,8 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print - if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); + SimpleFOCDebug::enable(&print); + SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); } // utility function intended to be used with serial plotter to monitor motor variables @@ -105,14 +110,11 @@ void FOCMotor::monitor() { } // 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 diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 37c5b260..670c69b2 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 */ @@ -137,6 +152,7 @@ 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) // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter @@ -145,6 +161,7 @@ class FOCMotor // motor physical parameters float phase_resistance; //!< motor phase resistance int pole_pairs;//!< motor pole pairs number + float KV_rating; //!< motor KV rating // limiting variables float voltage_limit; //!< Voltage limitting variable - global limit @@ -153,6 +170,7 @@ class FOCMotor // motor status vairables int8_t enabled = 0;//!< enabled or disabled motor flag + FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status // pwm modulation related variables FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index efab1be1..eb9608e8 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -6,19 +6,19 @@ /** * Direction structure */ -enum Direction{ - CW = 1, //clockwise +enum Direction : int8_t { + CW = 1, // clockwise CCW = -1, // counter clockwise - UNKNOWN = 0 //not yet known or invalid state + UNKNOWN = 0 // not yet known or invalid state }; /** * Pullup configuration structure */ -enum Pullup{ - USE_INTERN, //!< Use internal pullups - USE_EXTERN //!< Use external pullups +enum Pullup : uint8_t { + USE_INTERN = 0x00, //!< Use internal pullups + USE_EXTERN = 0x01 //!< Use external pullups }; /** diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 33f08843..2006fc8c 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,6 +1,8 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H +#include "drivers/hardware_api.h" + class StepperDriver{ public: @@ -14,7 +16,10 @@ class StepperDriver{ long pwm_frequency; //!< pwm frequency value in hertz float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor - + + bool initialized = false; // true if driver was successfully initialized + void* params = 0; // pointer to hardware specific parameters of driver + /** * Set phase voltages to the harware * diff --git a/src/common/defaults.h b/src/common/defaults.h index df651e2a..c0a46182 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -28,7 +28,7 @@ #define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e0358750..0d72485d 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -24,11 +24,13 @@ #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 _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 +#define _NC (NOT_SET) // dq current structure struct DQCurrent_s diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 8e1cb051..84bee9ba 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; @@ -102,9 +101,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) { // if target setting if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){ - printVerbose(F("Target: ")); - motor->target = atof(user_command); - println(motor->target); + target(motor, user_command); return; } @@ -178,59 +175,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 +212,23 @@ 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_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 | ")); @@ -379,6 +331,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 +465,95 @@ 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])) 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) diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 13777867..51647f71 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -12,10 +12,10 @@ // Commander verbose display to the user type -enum VerboseMode{ - nothing = 0, // display nothing - good for monitoring - on_request, // display only on user request - user_friendly // display textual messages to the user +enum VerboseMode : uint8_t { + nothing = 0x00, // display nothing - good for monitoring + on_request = 0x01, // display only on user request + user_friendly = 0x02 // display textual messages to the user }; @@ -100,9 +100,13 @@ class Commander Stream* com_port = nullptr; //!< Serial terminal variable if provided char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) + /** * * FOC motor (StepperMotor and BLDCMotor) command interface + * @param motor - FOCMotor (BLDCMotor or StepperMotor) instance + * @param user_cmd - the string command + * * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) * 'D' - D current PID controller & LPF (see function pid and lpf for commands) @@ -139,7 +143,11 @@ class Commander * 'C' - clear monitor * 'S' - set monitoring variables * 'G' - get variable value - * '' - Target get/set + * '' - Target setting interface + * Depends of the motion control mode: + * - torque : torque (ex. M2.5) + * - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits) + * - angle (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits) * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) @@ -149,6 +157,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 +167,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,11 +182,62 @@ 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 *)" "); private: // Subscribed command callback variables diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp new file mode 100644 index 00000000..e969d8a2 --- /dev/null +++ b/src/communication/SimpleFOCDebug.cpp @@ -0,0 +1,104 @@ + +#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 __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(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..614e6371 --- /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. + * + **/ + + +#ifndef SIMPLEFOC_DISABLE_DEBUG + +class SimpleFOCDebug { +public: + static void enable(Print* debugPrint = &Serial); + + static void println(const __FlashStringHelper* msg); + static void println(const char* msg); + static void println(const __FlashStringHelper* msg, float val); + static void println(const char* msg, float val); + static void println(const __FlashStringHelper* msg, int val); + static void println(const char* msg, int val); + static void println(); + static void println(int val); + static void println(float val); + + static void print(const char* msg); + static void print(const __FlashStringHelper* msg); + static void print(int val); + static void print(float val); + +protected: + static Print* _debugPrint; +}; + + +#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/commands.h b/src/communication/commands.h index 3ba6cdde..2dbc9d94 100644 --- a/src/communication/commands.h +++ b/src/communication/commands.h @@ -15,6 +15,7 @@ #define CMD_SENSOR 'S' //!< sensor offsets #define CMD_MONITOR 'M' //!< monitoring #define CMD_RESIST 'R' //!< motor phase resistance + #define CMD_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..269e0ba5 --- /dev/null +++ b/src/current_sense/GenericCurrentSense.cpp @@ -0,0 +1,159 @@ +#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(); + // 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 +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int GenericCurrentSense::driverAlign(float voltage){ + _UNUSED(voltage) ; // remove unused parameter warning + int exit_flag = 1; + if(skip_align) return exit_flag; + + // // set phase A active and phases B and C down + // driver->setPwm(voltage, 0, 0); + // _delay(200); + // PhaseCurrent_s c = getPhaseCurrents(); + // // read the current 100 times ( arbitrary number ) + // for (int i = 0; i < 100; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.a = c.a*0.6f + 0.4f*c1.a; + // c.b = c.b*0.6f + 0.4f*c1.b; + // c.c = c.c*0.6f + 0.4f*c1.c; + // _delay(3); + // } + // driver->setPwm(0, 0, 0); + // // align phase A + // float ab_ratio = fabs(c.a / c.b); + // float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + // if( ab_ratio > 1.5f ){ // should be ~2 + // gain_a *= _sign(c.a); + // }else if( ab_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and B + // int tmp_pinA = pinA; + // pinA = pinB; + // pinB = tmp_pinA; + // gain_a *= _sign(c.b); + // exit_flag = 2; // signal that pins have been switched + // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and C + // int tmp_pinA = pinA; + // pinA = pinC; + // pinC= tmp_pinA; + // gain_a *= _sign(c.c); + // exit_flag = 2;// signal that pins have been switched + // }else{ + // // error in current sense - phase either not measured or bad connection + // return 0; + // } + + // // set phase B active and phases A and C down + // driver->setPwm(0, voltage, 0); + // _delay(200); + // c = getPhaseCurrents(); + // // read the current 50 times + // for (int i = 0; i < 100; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.a = c.a*0.6f + 0.4f*c1.a; + // c.b = c.b*0.6f + 0.4f*c1.b; + // c.c = c.c*0.6f + 0.4f*c1.c; + // _delay(3); + // } + // driver->setPwm(0, 0, 0); + // float ba_ratio = fabs(c.b/c.a); + // float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + // if( ba_ratio > 1.5f ){ // should be ~2 + // gain_b *= _sign(c.b); + // }else if( ba_ratio < 0.7f ){ // it should be ~0.5 + // // switch phase A and B + // int tmp_pinB = pinB; + // pinB = pinA; + // pinA = tmp_pinB; + // gain_b *= _sign(c.a); + // exit_flag = 2; // signal that pins have been switched + // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + // // switch phase A and C + // int tmp_pinB = pinB; + // pinB = pinC; + // pinC = tmp_pinB; + // gain_b *= _sign(c.c); + // exit_flag = 2; // signal that pins have been switched + // }else{ + // // error in current sense - phase either not measured or bad connection + // return 0; + // } + + // // if phase C measured + // if(_isset(pinC)){ + // // set phase B active and phases A and C down + // driver->setPwm(0, 0, voltage); + // _delay(200); + // c = getPhaseCurrents(); + // // read the adc voltage 500 times ( arbitrary number ) + // for (int i = 0; i < 50; i++) { + // PhaseCurrent_s c1 = getPhaseCurrents(); + // c.c = (c.c+c1.c)/50.0f; + // } + // driver->setPwm(0, 0, 0); + // gain_c *= _sign(c.c); + // } + + // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + return exit_flag; +} diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h new file mode 100644 index 00000000..a63df49d --- /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) 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..86f81a47 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -20,46 +20,45 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ } // Inline sensor init function -void InlineCurrentSense::init(){ +int InlineCurrentSense::init(){ // configure ADC variables - _configureADCInline(pinA,pinB,pinC); + params = _configureADCInline(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); + // return success + return 1; } // Function finding zero offsets of the ADC void InlineCurrentSense::calibrateOffsets(){ const int calibration_rounds = 1000; - + // find adc offset = zero current voltage offset_ia = 0; offset_ib = 0; offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltageInline(pinA); - offset_ib += _readADCVoltageInline(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); + 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 @@ -69,96 +68,131 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ +int InlineCurrentSense::driverAlign(float voltage){ + int exit_flag = 1; if(skip_align) return exit_flag; - // 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; + if(_isset(pinA)){ + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = c.b ? fabs(c.a / c.b) : 0; + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + 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(_isset(pinB)){ + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = c.a ? fabs(c.b / c.a) : 0; + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + 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 + // set phase C active and phases A and B down driver->setPwm(0, 0, voltage); _delay(200); - c = getPhaseCurrents(); + PhaseCurrent_s c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0f; + 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); - gain_c *= _sign(c.c); + float ca_ratio = c.a ? fabs(c.c / c.a) : 0; + float cb_ratio = c.b ? fabs(c.c / c.b) : 0; + if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and C + int tmp_pinC = pinC; + pinC = pinA; + pinA = tmp_pinC; + gain_c *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 + // switch phase B and C + int tmp_pinC = pinC; + pinC = pinB; + pinB = tmp_pinC; + gain_c *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } } if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; @@ -168,5 +202,6 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 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..9ee25740 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -23,10 +23,9 @@ class InlineCurrentSense: public CurrentSense{ InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; + int driverAlign(float align_voltage) override; // ADC measuremnet gain for each phase // support for different gains for different phases of more commonly - inverted phase currents diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index ca4f51b4..8b9fe3ed 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -20,13 +20,17 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int } // Lowside sensor init function -void LowsideCurrentSense::init(){ +int LowsideCurrentSense::init(){ // configure ADC variables - _configureADCLowSide(pinA,pinB,pinC); + params = _configureADCLowSide(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(); + _driverSyncLowSide(driver->params, params); // calibrate zero offsets calibrateOffsets(); + // return success + return 1; } // Function finding zero offsets of the ADC void LowsideCurrentSense::calibrateOffsets(){ @@ -39,14 +43,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,17 +58,11 @@ void LowsideCurrentSense::calibrateOffsets(){ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; _startADC3PinConversionLowSide(); - current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + current.a = (!_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 @@ -74,97 +72,131 @@ int LowsideCurrentSense::driverSync(BLDCDriver *driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ +int LowsideCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; - // 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; + if(_isset(pinA)){ + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = c.b ? fabs(c.a / c.b) : 0; + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + 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(_isset(pinB)){ + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = c.a ? fabs(c.b / c.a) : 0; + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + 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 + // set phase C active and phases A and B down driver->setPwm(0, 0, voltage); _delay(200); - c = getPhaseCurrents(); + PhaseCurrent_s c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0f; + 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); - gain_c *= _sign(c.c); + float ca_ratio = c.a ? fabs(c.c / c.a) : 0; + float cb_ratio = c.b ? fabs(c.c / c.b) : 0; + if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and C + int tmp_pinC = pinC; + pinC = pinA; + pinA = tmp_pinC; + gain_c *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 + // switch phase B and C + int tmp_pinC = pinC; + pinC = pinB; + pinB = tmp_pinC; + gain_c *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } } if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 057aa623..654e0d45 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -21,13 +21,12 @@ 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); // CurrentSense interface implementing functions - void init() override; + int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverSync(BLDCDriver *driver) override; - int driverAlign(BLDCDriver *driver, float voltage) override; + int driverAlign(float align_voltage) override; // ADC measuremnet gain for each phase // support for different gains for different phases of more commonly - inverted phase currents @@ -41,6 +40,9 @@ class LowsideCurrentSense: public CurrentSense{ // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) private: // hardware variables @@ -57,9 +59,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..7862b708 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -4,30 +4,46 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +// flag returned if current sense init fails +#define SIMPLEFOC_CURRENT_SENSE_INIT_FAILED ((void*)-1) + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct GenericCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; +} GenericCurrentSenseParams; + + /** * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageInline(const int pinA); +float _readADCVoltageInline(const int pinA, const void* cs_params); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCInline(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); /** * function reading an ADC value and returning the read voltage * + * @param driver_params - driver parameter structure - hardware specific * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); +void* _configureADCLowSide(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET); void _startADC3PinConversionLowSide(); @@ -35,12 +51,15 @@ void _startADC3PinConversionLowSide(); * function reading an ADC value and returning the read voltage * * @param pinA - the arduino pin to be read (it has to be ADC pin) + * @param cs_params -current sense parameter structure - hardware specific */ -float _readADCVoltageLowSide(const int pinA); +float _readADCVoltageLowSide(const int pinA, const void* cs_params); /** * function syncing the Driver with the ADC for the LowSide Sensing + * @param driver_params - driver parameter structure - hardware specific + * @param cs_params - current sense parameter structure - hardware specific */ -void _driverSyncLowSide(); +void _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 75265c03..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_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp similarity index 83% rename from src/current_sense/hardware_specific/esp32_adc_driver.cpp rename to src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 76ee54e9..93aba630 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) #include "freertos/FreeRTOS.h" #include "freertos/task.h" @@ -216,10 +216,44 @@ void __analogReadResolution(uint8_t bits) uint16_t IRAM_ATTR adcRead(uint8_t pin) { - if(!__adcAttachPin(pin) || !__adcStart(pin)){ - return 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin } - return __adcEnd(pin); + + __analogInit(); + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + } + + uint16_t value = 0; + + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h similarity index 95% rename from src/current_sense/hardware_specific/esp32_adc_driver.h rename to src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index df8166f0..869c4dde 100644 --- a/src/current_sense/hardware_specific/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp new file mode 100644 index 00000000..f2d65f8e --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -0,0 +1,27 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) + +#include "esp32_adc_driver.h" + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp new file mode 100644 index 00000000..941fb385 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -0,0 +1,164 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_specific/esp32_driver_mcpwm.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) + +#include "esp32_adc_driver.h" + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#include +#include + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + + +typedef struct ESP32MCPWMCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + mcpwm_unit_t mcpwm_unit; + int buffer_index; +} ESP32MCPWMCurrentSenseParams; + + +/** + * 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 * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + _UNUSED(driver_params); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + + + +/** + * Low side adc reading implementation +*/ + +static void IRAM_ATTR mcpwm0_isr_handler(void*); +static void IRAM_ATTR mcpwm1_isr_handler(void*); +byte currentState = 1; +// two mcpwm units +// - max 2 motors per mcpwm unit (6 adc channels) +int adc_pins[2][6]={0}; +int adc_pin_count[2]={0}; +uint32_t adc_buffer[2][6]={0}; +int adc_read_index[2]={0}; + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint32_t raw_adc; + + mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; + int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; + float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; + + for(int i=0; i < adc_pin_count[unit]; i++){ + if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; + } + // not found + return 0; +} + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + + mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + int index_start = adc_pin_count[unit]; + if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; + if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; + if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), + .mcpwm_unit = unit, + .buffer_index = index_start + }; + + return params; +} + + +void _driverSyncLowSide(void* driver_params, void* cs_params){ + + mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; + mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + + // low-side register enable interrupt + mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // high side registers enable interrupt + //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt + + // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) + if(mcpwm_unit == MCPWM_UNIT_0) + mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler + else + mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm0_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; + + // low side + uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); + adc_read_index[0]++; + if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; + } + // low side + MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; +} + + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm1_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; + + // low side + uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); + adc_read_index[1]++; + if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; + } + // low side + MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32_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..dec7205d 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,31 +1,41 @@ #include "../hardware_api.h" // function reading an ADC value and returning the read voltage -__attribute__((weak)) float _readADCVoltageInline(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * 5.0f/1024.0f; + return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv; } // function reading an ADC value and returning the read voltage -__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ - 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){ + return _readADCVoltageInline(pinA, cs_params); } // Configure low side for generic mcu // cannot do much but -__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); +__attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + return _configureADCInline(driver_params, pinA, pinB, pinC); } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(){ } +__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ + _UNUSED(driver_params); + _UNUSED(cs_params); +} __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/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..da5ba85b 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,8 +54,11 @@ float _readADCVoltageLowSide(const int pinA) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide() +void _driverSyncLowSide(void* driver_params, void* cs_params) { + _UNUSED(driver_params); + _UNUSED(cs_params); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts @@ -137,7 +144,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 +177,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 98% 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..806c68e7 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,10 @@ -#include "../hardware_api.h" -#if defined(STM32G4xx) +#include "../../../hardware_api.h" +#if defined(ARDUINO_B_G431B_ESC1) #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 /** ****************************************************************************** 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/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp similarity index 65% rename from src/current_sense/hardware_specific/stm32g4_mcu.cpp rename to src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 5b5d00ee..4adfff57 100644 --- a/src/current_sense/hardware_specific/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -1,9 +1,14 @@ -#include "../hardware_api.h" -#include "stm32g4_hal.h" +#include "../../../hardware_api.h" -#if defined(STM32G4xx) -#define _ADC_VOLTAGE 3.3 -#define _ADC_RESOLUTION 4096.0 +#if defined(ARDUINO_B_G431B_ESC1) + +#include "b_g431_hal.h" +#include "Arduino.h" +#include "../stm32_mcu.h" +#include "../../../../drivers/hardware_specific/stm32_mcu.h" + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f #define ADC_BUF_LEN_1 2 #define ADC_BUF_LEN_2 1 @@ -19,24 +24,19 @@ static DMA_HandleTypeDef hdma_adc2; uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin){ +float _readADCVoltageInline(const int pin, const void* cs_params){ uint32_t raw_adc = 0; if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[1]; 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]; - - return raw_adc * _ADC_CONV; -} -// do the same for low side sensing -float _readADCVoltageLowSide(const int pin){ - return _readADCVoltageInline(pin); +#endif + return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ @@ -80,7 +80,9 @@ void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Chan __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); } -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + HAL_Init(); MX_GPIO_Init(); MX_DMA_Init(); @@ -109,12 +111,15 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. _delay(5); if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { - Error_Handler(); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } -} -// do the same for low side -void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ - _configureADCInline(pinA, pinB, pinC); + + Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + + return params; } extern "C" { @@ -127,4 +132,27 @@ void DMA1_Channel2_IRQHandler(void) { } } +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); + +} + #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..51300194 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -0,0 +1,162 @@ +#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_EXTERNALTRIGINJECCONV_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 = 0; + 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); + } + + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + 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..5eb45049 --- /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_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..5015c058 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -0,0 +1,104 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F1xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/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}; + +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={0}, + .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; + + // 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 + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + 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..ac890817 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -0,0 +1,170 @@ +#include "stm32f4_hal.h" + +#if defined(STM32F4xx) + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + + +// 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; +} + +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.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; + 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_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 = 1; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + // second channel + sConfigInjected.InjectedRank = 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 = 3; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + } + + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + 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..35b5aa67 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -0,0 +1,17 @@ +#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_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/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp new file mode 100644 index 00000000..f764f65c --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -0,0 +1,106 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F4xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32f4_hal.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}; + +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={0}, + .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; + + // 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 + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + 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_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp new file mode 100644 index 00000000..10587bd1 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -0,0 +1,238 @@ +#include "stm32g4_hal.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + + +// 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 == TIM2) + 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 == TIM2) + 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; +} + +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) { +#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 + 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.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; + 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_2CYCLES_5; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_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 = 1; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + // second channel + sConfigInjected.InjectedRank = 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 = 3; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); + HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + } + + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + cs_params->adc_handle = &hadc; + return 0; +} + +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/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h new file mode 100644 index 00000000..c4159c72 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -0,0 +1,18 @@ +#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_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/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp new file mode 100644 index 00000000..254216f3 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -0,0 +1,107 @@ +#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_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32g4_hal.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE_G4 3.3f +#define _ADC_RESOLUTION_G4 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}; + +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={0}, + .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; + + // 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 + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + 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_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/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp index 606ce618..7ab370a4 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -5,33 +5,20 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ - uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; -} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ - 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/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index bc239875..5115cf37 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -56,8 +56,9 @@ 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; } @@ -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..aaf38c7b 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -58,7 +58,6 @@ class BLDCDriver3PWM: public BLDCDriver */ virtual void setPhaseState(int sa, int sb, int sc) override; private: - }; diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index eaeb2cfa..1fe41528 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -58,9 +58,12 @@ int BLDCDriver6PWM::init() { // configure 6pwm // hardware specific function - depending on driver and mcu - return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + 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 +77,14 @@ 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, params); } // Set voltage to the pwm pin void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { + _UNUSED(sa); + _UNUSED(sb); + _UNUSED(sc); // TODO implement disabling } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a0e61ca0..dbbf5b8f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -79,8 +79,9 @@ 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; } @@ -102,5 +103,5 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); // write to hardware - _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params); } \ No newline at end of file diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 6c905348..836f5472 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -54,8 +54,9 @@ 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; } @@ -76,5 +77,5 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { else duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware - _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); } \ No newline at end of file diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 0b4f7c65..cb2385b7 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -3,6 +3,22 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../communication/SimpleFOCDebug.h" + + +// flag returned if driver init fails +#define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) + +// 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 @@ -12,8 +28,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 +42,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 +57,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 +76,9 @@ 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()) @@ -65,10 +87,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 +99,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 +112,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 +125,9 @@ 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 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, void* params); #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 2d3c0305..cf71a949 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -28,64 +28,79 @@ void _pinHighFrequency(const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -122,14 +137,20 @@ int _configureComplementaryPair(int pinH, int pinL) { // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + .dead_zone = dead_zone + }; + return params; } // function setting the @@ -149,10 +170,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 2c64ee64..33e4d497 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -21,68 +21,81 @@ void _pinHighFrequency(const int pin){ // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328 -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { - _UNUSED(pwm_frequency); +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic // supports Arudino/ATmega328 -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - _UNUSED(pwm_frequency); +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +// supports Arduino/ATmega328 +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -115,19 +128,23 @@ int _configureComplementaryPair(int pinH, int pinL) { } // Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting +// - BLDC driver - setting // - hardware specific // supports Arudino/ATmega328 -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + return params; } // function setting the @@ -147,10 +164,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 5a8abe2e..22b6c656 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -31,64 +31,79 @@ void _pinHighFrequency(const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); _pinHighFrequency(pinB); _pinHighFrequency(pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } @@ -134,17 +149,21 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - _UNUSED(pwm_frequency); - _UNUSED(dead_zone); +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz int ret_flag = 0; ret_flag += _configureComplementaryPair(pinA_h, pinA_l); ret_flag += _configureComplementaryPair(pinB_h, pinB_l); ret_flag += _configureComplementaryPair(pinC_h, pinC_l); - return ret_flag; // returns -1 if not well configured - + if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED; + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA_h,, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency + .dead_zone = dead_zone + }; + return params; +} // function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) @@ -163,10 +182,10 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // - BLDC driver - 6PWM setting // - hardware specific // supports Arudino/ATmega328 -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); - _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); - _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); + _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0); } #endif diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index e0d39afd..80ef82d9 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -320,13 +320,17 @@ void TC8_Handler() if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]); } + + + + // implementation of the hardware_api.cpp // --------------------------------------------------------------------------------------------------------------------------------- // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware specific -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -337,13 +341,21 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinC, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } + + // Configuring PWM frequency, resolution and alignment //- Stepper driver - 2PWM setting // - hardware specific -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -353,12 +365,21 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { initPWM(pinB, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency @@ -370,36 +391,52 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int initPWM(pinD, _pwm_frequency); // sync the timers if possible syncTimers(pinA, pinB, pinC, pinD); + + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); - setPwm(pinC, _max_pwm_value*dc_c); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); + setPwm(p->pins[2], _max_pwm_value*dc_c); } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pin1A, _max_pwm_value*dc_1a); - setPwm(pin1B, _max_pwm_value*dc_1b); - setPwm(pin2A, _max_pwm_value*dc_2a); - setPwm(pin2B, _max_pwm_value*dc_2b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_1a); + setPwm(p->pins[1], _max_pwm_value*dc_1b); + setPwm(p->pins[2], _max_pwm_value*dc_2a); + setPwm(p->pins[3], _max_pwm_value*dc_2b); } + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - Stepper driver - 2PWM setting // - hardware specific -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* param){ // transform duty cycle from [0,1] to [0,_max_pwm_value] - setPwm(pinA, _max_pwm_value*dc_a); - setPwm(pinB, _max_pwm_value*dc_b); + GenericDriverParams* p = (GenericDriverParams*)param; + setPwm(p->pins[0], _max_pwm_value*dc_a); + setPwm(p->pins[1], _max_pwm_value*dc_b); } diff --git a/src/drivers/hardware_specific/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32_driver_mcpwm.h new file mode 100644 index 00000000..1505072c --- /dev/null +++ b/src/drivers/hardware_specific/esp32_driver_mcpwm.h @@ -0,0 +1,88 @@ +#ifndef ESP32_DRIVER_MCPWM_H +#define ESP32_DRIVER_MCPWM_H + +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +// empty motor slot +#define _EMPTY_SLOT -20 +#define _TAKEN_SLOT -21 + +// ABI bus frequency - would be better to take it from somewhere +// but I did nto find a good exposed variable +#define _MCPWM_FREQ 160e6f + +// preferred pwm resolution default +#define _PWM_RES_DEF 4096 +// min resolution +#define _PWM_RES_MIN 3000 +// max resolution +#define _PWM_RES_MAX 8000 +// pwm frequency +#define _PWM_FREQUENCY 25000 // default +#define _PWM_FREQUENCY_MAX 50000 // mqx + +// structure containing motor slot configuration +// this library supports up to 4 motors +typedef struct { + int pinA; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; + mcpwm_io_signals_t mcpwm_c; +} bldc_3pwm_motor_slots_t; + +typedef struct { + int pin1A; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_1a; + mcpwm_io_signals_t mcpwm_1b; + mcpwm_io_signals_t mcpwm_2a; + mcpwm_io_signals_t mcpwm_2b; +} stepper_4pwm_motor_slots_t; + +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + + +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; + mcpwm_dev_t* mcpwm_dev; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; +} ESP32MCPWMDriverParams; + + +#endif +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp new file mode 100644 index 00000000..55348e2b --- /dev/null +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -0,0 +1,148 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) + +#include "driver/ledc.h" + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution +#define _PWM_RES_BIT 10 // 10 bir resolution +#define _PWM_RES 1023 // 2^10-1 = 1023-1 + + +// empty motor slot +#define _EMPTY_SLOT -20 +#define _TAKEN_SLOT -21 + +// figure out how many ledc channels are avaible +// esp32 - 2x8=16 +// esp32s2 - 8 +// esp32c3 - 6 +#include "soc/soc_caps.h" +#ifdef SOC_LEDC_SUPPORT_HS_MODE +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1) +#else +#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) +#endif + + +// current channel stack index +// support for multiple motors +// esp32 has 16 channels +// esp32s2 has 8 channels +// esp32c3 has 6 channels +int channel_index = 0; + + + + +typedef struct ESP32LEDCDriverParams { + int channels[6]; + long pwm_frequency; +} ESP32LEDCDriverParams; + + + + + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin, const int channel){ + ledcSetup(channel, freq, _PWM_RES_BIT ); + ledcAttachPin(pin, channel); +} + + + +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + int ch3 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + _setHighFrequency(pwm_frequency, pinC, ch3); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2, ch3 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + int ch2 = channel_index++; + int ch3 = channel_index++; + int ch4 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + _setHighFrequency(pwm_frequency, pinB, ch2); + _setHighFrequency(pwm_frequency, pinC, ch3); + _setHighFrequency(pwm_frequency, pinD, ch4); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1, ch2, ch3, ch4 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); +} + + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); +} + + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index f6663d87..22719787 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,73 +1,6 @@ -#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; +#include "esp32_driver_mcpwm.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) // define bldc motor slots array bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { @@ -97,9 +30,13 @@ stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B }; + // configuring high frequency pwm timer // a lot of help from this post from Paul Gauld // https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +// a short tutorial for v2.0.1+ +// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html +// void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ mcpwm_config_t pwm_config; @@ -124,7 +61,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.prescale = 0; + mcpwm_num->clk_cfg.clk_prescale = 0; // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 @@ -140,18 +77,18 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler - mcpwm_num->timer[0].period.prescale = prescaler; - mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; + mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; + mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; _delay(1); //set period - mcpwm_num->timer[0].period.period = resolution_corrected; - mcpwm_num->timer[1].period.period = resolution_corrected; - mcpwm_num->timer[2].period.period = resolution_corrected; + mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; + mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; + mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; _delay(1); - mcpwm_num->timer[0].period.upmethod = 0; - mcpwm_num->timer[1].period.upmethod = 0; - mcpwm_num->timer[2].period.upmethod = 0; + mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; + mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; + mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; _delay(1); // _delay(1); //restart the timers @@ -159,23 +96,25 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); _delay(1); - // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined - // in the default Espressif MCPWM headers. The correct const may be used - // when https://github.com/espressif/esp-idf/issues/5429 is resolved. - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, (mcpwm_sync_signal_t)1, 0); - mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, (mcpwm_sync_signal_t)1, 0); - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 1; - _delay(1); - mcpwm_num->timer[0].sync.out_sel = 0; + + mcpwm_sync_config_t sync_conf = { + .sync_sig = MCPWM_SELECT_TIMER0_SYNC, + .timer_val = 0, + .count_direction = MCPWM_TIMER_DIRECTION_UP + }; + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); + mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); + + // Enable sync event for all timers to be the TEZ of timer0 + mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +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 @@ -213,6 +152,13 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator + }; + return params; } @@ -220,7 +166,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - BLDC motor - 3PWM setting // - hardware speciffic // supports Arudino/ATmega328, STM32 and ESP32 -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max @@ -258,13 +204,20 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator + }; + return params; } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +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 = {}; @@ -307,66 +260,69 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator1, + .mcpwm_operator2 = m_slot.mcpwm_operator2 + }; + return params; } + + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - break; - } - } +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); } + + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 4; i++){ - if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); - break; - } - } +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); } + + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic // ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); - break; - } - } +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); } + + + // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency @@ -382,7 +338,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const } } // if no slots available - if(slot_num >= 2) return -1; + if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; // disable all the slots with the same MCPWM if( slot_num == 0 ){ @@ -410,26 +366,29 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); // return - return 0; + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { + .pwm_frequency = pwm_frequency, + .mcpwm_dev = m_slot.mcpwm_num, + .mcpwm_unit = m_slot.mcpwm_unit + }; + return params; } + + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to - for(int i = 0; i < 2; i++){ - if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ // se the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - break; - } - } + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); } + #endif diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index ccc31aa2..df8ae380 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -15,63 +15,78 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pin1A, pin2A, pin2A, pin2B }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index f89c0562..cb3bfd34 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -9,23 +9,24 @@ // - 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 +34,69 @@ __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 - 2PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ _UNUSED(dc_a); _UNUSED(dc_b); _UNUSED(dc_c); - _UNUSED(dead_zone); - _UNUSED(pinA_h); - _UNUSED(pinB_h); - _UNUSED(pinC_h); - _UNUSED(pinA_l); - _UNUSED(pinB_l); - _UNUSED(pinC_l); - return; + _UNUSED(params); } diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp new file mode 100644 index 00000000..2dcadc4f --- /dev/null +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -0,0 +1,391 @@ + +#include "../hardware_api.h" + +#if defined(NRF52_SERIES) + + +#define PWM_CLK (16000000) +#define PWM_FREQ (40000) +#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ) +#define PWM_MAX_FREQ (62500) +#define DEAD_ZONE (250) // in ns +#define DEAD_TIME (DEAD_ZONE / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM + +#ifdef NRF_PWM3 +#define PWM_COUNT 4 +#else +#define PWM_COUNT 3 +#endif + +// empty motor slot +#define _EMPTY_SLOT (-0xAA) +#define _TAKEN_SLOT (-0x55) + +int pwm_range; + + +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, 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; +} + + +#endif diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b314ee40..b426f0ed 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -21,13 +21,14 @@ // #define _SOFTWARE_6PWM 0 // #define _ERROR_6PWM -1 -typedef struct{ - int id ; - pwmout_t pins[6]; -} portenta_h7_mcu_enty_s; -portenta_h7_mcu_enty_s motor_slot[10]; -int slot_index = 0; + +typedef struct PortentaDriverParams { + pwmout_t pins[4]; + long pwm_frequency; +// float dead_zone; +} PortentaDriverParams; + /* Convert STM32 Cube HAL channel to LL channel */ @@ -379,112 +380,111 @@ void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1])); core_util_critical_section_exit(); - slot_index++; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - motor_slot[slot_index].id = pinA; + PortentaDriverParams* params = new PortentaDriverParams(); + params->pwm_frequency = pwm_frequency; + core_util_critical_section_enter(); - _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); - _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency); + _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(params->pins[3]), pinD, (long)pwm_frequency); // allign the timers - _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3])); + _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2]), &(params->pins[3])); core_util_critical_section_exit(); - slot_index++; + + return params; } + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting //- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ -for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - for(int i=0; ipins[0]), (float)dc_a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_c); + core_util_critical_section_exit(); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - for(int i=0; ipins[0]), (float)dc_1a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_1b); + _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_2a); + _pwm_write(&(((PortentaDriverParams*)params)->pins[3]), (float)dc_2b); + core_util_critical_section_exit(); } - +// 6-PWM currently not supported, defer to generic, which also doesn't support it ;-) // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +//void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ // if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz // else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // // center-aligned frequency is uses two periods @@ -510,13 +510,13 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // _alignPWMTimers(HT1, HT2, HT3); // break; // } - return -1; // success -} +// return -1; // success +// } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +//void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ // // find configuration // int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); // // set pwm accordingly @@ -535,5 +535,5 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); // break; // } -} +//} #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 7be8faaf..2c127d71 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -19,6 +19,14 @@ +typedef struct RP2040DriverParams { + int pins[6]; + uint slice[6]; + uint chan[6]; + long pwm_frequency; + float dead_zone; +} RP2040DriverParams; + // until I can figure out if this can be quickly read from some register, keep it here. // it also serves as a marker for what slices are already used. @@ -27,10 +35,13 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert = false) { +void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); + params->pins[index] = pin; + params->slice[index] = slice; + params->chan[index] = chan; pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get pwm_set_phase_correct(slice, true); uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; @@ -61,7 +72,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert = false) { void syncSlices() { - for (int i=0;ipwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, false, params, 0); + setupPWM(pinB, pwm_frequency, false, params, 1); syncSlices(); + return params; } -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { - setupPWM(pinA, pwm_frequency); - setupPWM(pinB, pwm_frequency); - setupPWM(pinC, pwm_frequency); +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, false, params, 0); + setupPWM(pinB, pwm_frequency, false, params, 1); + setupPWM(pinC, pwm_frequency, false, params, 2); syncSlices(); + return params; } -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { - setupPWM(pin1A, pwm_frequency); - setupPWM(pin1B, pwm_frequency); - setupPWM(pin2A, pwm_frequency); - setupPWM(pin2B, pwm_frequency); +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + setupPWM(pin1A, pwm_frequency, false, params, 0); + setupPWM(pin1B, pwm_frequency, false, params, 1); + setupPWM(pin2A, pwm_frequency, false, params, 2); + setupPWM(pin2B, pwm_frequency, false, params, 3); syncSlices(); + return params; } -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // non-PIO solution... - setupPWM(pinA_h, pwm_frequency); - setupPWM(pinB_h, pwm_frequency); - setupPWM(pinC_h, pwm_frequency); - setupPWM(pinA_l, pwm_frequency, true); - setupPWM(pinB_l, pwm_frequency, true); - setupPWM(pinC_l, pwm_frequency, true); + RP2040DriverParams* params = new RP2040DriverParams(); + params->pwm_frequency = pwm_frequency; + params->dead_zone = dead_zone; + setupPWM(pinA_h, pwm_frequency, false, params, 0); + setupPWM(pinB_h, pwm_frequency, false, params, 2); + setupPWM(pinC_h, pwm_frequency, false, params, 4); + setupPWM(pinA_l, pwm_frequency, true, params, 1); + setupPWM(pinB_l, pwm_frequency, true, params, 3); + setupPWM(pinC_l, pwm_frequency, true, params, 5); syncSlices(); - return 0; + return params; } -void writeDutyCycle(float val, int pin) { - uint slice = pwm_gpio_to_slice_num(pin); - uint chan = pwm_gpio_to_channel(pin); +void writeDutyCycle(float val, uint slice, uint chan) { pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val); } @@ -123,26 +144,26 @@ void writeDutyCycle(float val, int pin) { -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { - writeDutyCycle(dc_a, pinA); - writeDutyCycle(dc_b, pinB); +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); } -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { - writeDutyCycle(dc_a, pinA); - writeDutyCycle(dc_b, pinB); - writeDutyCycle(dc_c, pinC); +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); } -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B) { - writeDutyCycle(dc_1a, pin1A); - writeDutyCycle(dc_1b, pin1B); - writeDutyCycle(dc_2a, pin2A); - writeDutyCycle(dc_2b, pin2B); +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) { + writeDutyCycle(dc_1a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(dc_1b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_2a, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + writeDutyCycle(dc_2b, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); } inline float swDti(float val, float dt) { @@ -151,13 +172,13 @@ inline float swDti(float val, float dt) { return ret; } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { - writeDutyCycle(dc_a, pinA_h); - writeDutyCycle(swDti(dc_a, dead_zone), pinA_l); - writeDutyCycle(dc_b, pinB_h); - writeDutyCycle(swDti(dc_b,dead_zone), pinB_l); - writeDutyCycle(dc_c, pinC_h); - writeDutyCycle(swDti(dc_c,dead_zone), pinC_l); +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); + writeDutyCycle(swDti(dc_a, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]); + writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]); + writeDutyCycle(swDti(dc_b, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]); + writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]); + writeDutyCycle(swDti(dc_c, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]); } #endif diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index ab41b745..d9bb5b9a 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -159,11 +159,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 +178,15 @@ void configureSAMDClock() { * faster won't be possible without sacrificing resolution. */ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { + + long pwm_resolution = (24000000) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_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 +243,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 +263,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.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 +290,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 +318,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 +337,7 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { // while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { - Tc* tc = (Tc*)GetTC(chaninfo); + Tc* tc = (Tc*)GetTC(info->tcc.chaninfo); tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index b59a43fc..a9191702 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -7,6 +7,16 @@ +// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency. +// for custom boards or overclockers you can override it using this define. +#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ +#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000 +#endif + + + + + // TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation // 0 6 8 24-bit Yes Yes Yes Yes Yes Yes // 1 4 8 24-bit Yes Yes Yes Yes Yes Yes @@ -17,7 +27,6 @@ #define NUM_WO_ASSOCIATIONS 72 - struct wo_association WO_associations[] = { { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, @@ -112,7 +121,7 @@ struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { EPioType getPeripheralOfPermutation(int permutation, int pin_position) { - return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; + return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT; } @@ -124,24 +133,17 @@ void syncTCC(Tcc* TCCx) { -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CCBUF - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! -// tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); -// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); -// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency! } - else { - // Tc* tc = (Tc*)GetTC(chaninfo); - // //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - // tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); - // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + else { + // we don't support the TC channels on SAMD51, isn't worth it. } } @@ -183,12 +185,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 +243,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this? while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -247,18 +258,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 +293,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("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_mcu.cpp index e1d38a80..4198cc9b 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -278,7 +278,7 @@ int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguratio * @param pinA pinA bldc driver * @param pinB pinB bldc driver */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +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,40 +734,42 @@ 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, 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; } @@ -746,85 +784,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); + SimpleFOCDebug::println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); + SimpleFOCDebug::print(": TC Peripheral"); else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); + SimpleFOCDebug::print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; + SimpleFOCDebug::print(" E "); break; case PIO_TIMER_ALT: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; + SimpleFOCDebug::print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; + SimpleFOCDebug::print(" G "); break; #endif default: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; + SimpleFOCDebug::print(" ? "); break; } if (info.tcc.tccn>=0) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SimpleFOCDebug::print(info.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(info.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(info.wo); + SimpleFOCDebug::println("]"); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); + SimpleFOCDebug::println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 7faf7442..181d8faf 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -3,11 +3,9 @@ #define SAMD_MCU_H -// uncomment to enable debug output to Serial port +// uncomment to enable debug output from SAMD driver +// can set this as build-flag in Arduino IDE or PlatformIO // #define SIMPLEFOC_SAMD_DEBUG -#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) -#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial -#endif #include "../hardware_api.h" @@ -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_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 540dd6c2..aedc4b3c 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -1,98 +1,116 @@ #include "../hardware_api.h" +#include "stm32_mcu.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 +#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]; + + + + // setting pwm to hardware pin - instead analogWrite() -void _setPwm(int ulPin, uint32_t value, int resolution) +void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution) { - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + // 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); } -// 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); + + + +// 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 *)pinmap_peripheral(pin, PinMap_PWM)); + 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(pinmap_function(pin, PinMap_PWM)); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); - HT->setOverflow(PWM_freq, HERTZ_FORMAT); + uint32_t channel = STM_PIN_CHANNEL(timer->function); HT->pause(); - HT->refresh(); + if (init) + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin); +#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, int ulPin) -{ - return _initPinPWM(PWM_freq, ulPin); +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { + return _initPinPWM(PWM_freq, timer); } // init low side pin -HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer) { - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); + 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 *)pinmap_peripheral(pin, PinMap_PWM)); + 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)); - 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); + init = true; } 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); + 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(); - HT->refresh(); + 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); return HT; } + // align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3) { HT1->pause(); HT1->refresh(); @@ -105,8 +123,11 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) HT3->resume(); } + + + // align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4) { HT1->pause(); HT1->refresh(); @@ -122,176 +143,515 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha HT4->resume(); } -// configure hardware 6pwm interface only one timer with inverted channels -HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) + +// 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 + } +} + +// align the timers to end the init +void _startTimers(HardwareTimer **timers_to_start, int timer_num) { + // TODO - sart each timer only once + // sart timers + for (int i=0; i < timer_num; i++) { + if(timers_to_start[i] == NP) return; + timers_to_start[i]->resume(); + } +} + +void _alignTimersNew() { + int numTimers = 0; + HardwareTimer *timers[numTimerPinsUsed]; + + // reset timer counters + for (int i=0; iperipheral); + HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + bool found = false; + for (int j=0; jpause(); + 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(); + } + +} + + + + +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; +} + + + + +// 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 - 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); +#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface + return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing +#endif - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + uint32_t channel1 = STM_PIN_CHANNEL(pinH->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinL->function); - uint32_t index = get_timer_index(Instance); + // 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 *)pinmap_peripheral(uhPinName, PinMap_PWM)); + 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(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)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); + 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); 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); + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); 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; + params->timers[paramsPos] = HT; + params->timers[paramsPos+1] = HT; + params->channels[paramsPos] = channel1; + params->channels[paramsPos+1] = channel2; + return params; +} - HT->resume(); - return HT; -#else - return nullptr; // return nothing -#endif + + + +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; } -// 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 + + +/* + 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; +}; + + + + + + + + + // 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 (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; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + 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]); // 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; } + + + // 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 (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; - 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); + 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]); + + 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) { +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; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + 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]); // 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 // - Stepper motor - 2PWM setting //- hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting //- hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting //- hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,4095] - _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); - _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); - _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); - _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); } @@ -300,56 +660,180 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if (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 config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + 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 - 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; + 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]); + 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 0; // success + return params; // success } + + + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // find configuration - int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - // set pwm accordingly - switch(config){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ + switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: - _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + float dead_zone = ((STM32DriverParams*)params)->dead_zone / 2.0f; + _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); break; } } -#endif \ No newline at end of file + + + + + +#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_mcu.h b/src/drivers/hardware_specific/stm32_mcu.h new file mode 100644 index 00000000..cb07ba1d --- /dev/null +++ b/src/drivers/hardware_specific/stm32_mcu.h @@ -0,0 +1,32 @@ +#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.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 + + +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); + +#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 index 67bb0ad1..93331252 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -15,63 +15,78 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA, pinB, pinC, pinD }, + .pwm_frequency = pwm_frequency + }; + return params; } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0f*dc_a); - analogWrite(pinB, 255.0f*dc_b); - analogWrite(pinC, 255.0f*dc_c); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0f*dc_1a); - analogWrite(pin1B, 255.0f*dc_1b); - analogWrite(pin2A, 255.0f*dc_2a); - analogWrite(pin2B, 255.0f*dc_2b); + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 7c164878..96057d82 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -205,6 +205,7 @@ void Encoder::init(){ // change it if the mode is quadrature if(quadrature == Quadrature::ON) cpr = 4*cpr; + // we don't call Sensor::init() here because init is handled in Encoder class. } // function enabling hardware interrupts of the for the callback provided diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 3e775338..91427a83 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -10,9 +10,9 @@ /** * Quadrature mode configuration structure */ -enum Quadrature{ - ON, //!< Enable quadrature mode CPR = 4xPPR - OFF //!< Disable quadrature mode / CPR = PPR +enum Quadrature : uint8_t { + ON = 0x00, //!< Enable quadrature mode CPR = 4xPPR + OFF = 0x01 //!< Disable quadrature mode / CPR = PPR }; class Encoder: public Sensor{ 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..3d2ba42e 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -166,6 +166,7 @@ void HallSensor::init(){ pulse_timestamp = _micros(); + // we don't call Sensor::init() here because init is handled in HallSensor class. } // function enabling hardware interrupts for the callback provided diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 6d881657..d4adad60 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -24,6 +24,8 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ raw_count = getRawCount(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 6c61b8ce..af93b8cc 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -64,6 +64,8 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // I2C communication begin wire->begin(); + + this->Sensor::init(); // call base class init } // Shaft angle calculation diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 5088fa9e..c043e7ce 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -27,6 +27,8 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + + this->Sensor::init(); // call base class init } // get current angle (rad) diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b7a9dd27..7341c89a 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -1,4 +1,3 @@ -#ifndef TARGET_RP2040 #include "MagneticSensorSPI.h" @@ -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..a77e7698 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -1,7 +1,6 @@ #ifndef MAGNETICSENSORSPI_LIB_H #define MAGNETICSENSORSPI_LIB_H -#ifndef TARGET_RP2040 #include "Arduino.h" #include @@ -83,4 +82,3 @@ class MagneticSensorSPI: public Sensor{ #endif -#endif